Insane Clown Posse

rigid_transform.py

#%%

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!"