Last active
June 8, 2017 18:34
-
-
Save maxvoxel8/56d881f2d59132d4894053ea2c0d891f to your computer and use it in GitHub Desktop.
Insane Clown Posse
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#%% | |
from numpy import * | |
from math import sqrt | |
# Input: expects Nx3 matrix of points | |
# Returns R,t | |
# R = 3x3 rotation matrix | |
# t = 3x1 column vector | |
def rigid_transform_3D(A, B): | |
assert len(A) == len(B) | |
N = A.shape[0]; # total points | |
centroid_A = mean(A, axis=0) | |
centroid_B = mean(B, axis=0) | |
# centre the points | |
AA = A - tile(centroid_A, (N, 1)) | |
BB = B - tile(centroid_B, (N, 1)) | |
# dot is matrix multiplication for array | |
H = transpose(AA) * BB | |
U, S, Vt = linalg.svd(H) | |
R = Vt.T * U.T | |
# special reflection case | |
if linalg.det(R) < 0: | |
print "Reflection detected" | |
Vt[2,:] *= -1 | |
R = Vt.T * U.T | |
t = -R*centroid_A.T + centroid_B.T | |
return R, t | |
def rotationMatrixToEulerAngles(R) : | |
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) | |
singular = sy < 1e-6 | |
if not singular : | |
x = math.atan2(R[2,1] , R[2,2]) | |
y = math.atan2(-R[2,0], sy) | |
z = math.atan2(R[1,0], R[0,0]) | |
else : | |
print "singular" | |
x = math.atan2(-R[1,2], R[1,1]) | |
y = math.atan2(-R[2,0], sy) | |
z = 0 | |
return array([x, y, z]) | |
test_points = mat([ | |
[0, 0, -37.447], | |
[0, -50, -38.206], | |
[0, -100, -45.221], | |
[0, -150, -44.535], | |
[0, -200, -37.793] | |
]) | |
actual_points = mat([ | |
[2.6, 0, -36.69], | |
[4.1, -49.94, -37.715], | |
[4.8, -99.66, -44.44], | |
[6.9, -150.36, -44.046], | |
[7.7, -200.26, -36.996] | |
]) | |
ret_R, ret_t = rigid_transform_3D(test_points, actual_points) | |
A2 = (ret_R*test_points.T) + tile(ret_t, (1, 5)) | |
A2 = A2.T | |
# Find the error | |
err = A2 - actual_points | |
err = multiply(err, err) | |
err = sum(err) | |
rmse = sqrt(err/len(actual_points)) | |
set_printoptions(precision=20) | |
print "Test points" | |
print test_points | |
print "" | |
print "Actual points" | |
print actual_points | |
print "" | |
print "Estimated Rotation" | |
print ret_R | |
print "" | |
print "Estimated Angles" | |
print rotationMatrixToEulerAngles(ret_R) * 180 / pi | |
print "" | |
print "Estimated Translation" | |
print ret_t | |
print "" | |
print "RMSE:", rmse | |
print "If RMSE is near zero, the function is correct!" |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment