svd_rigid_transform.py

import numpy as np
from numpy import linalg as la

"""
https://www.ltu.se/cms_fs/1.51590!/svd-fitting.pdf
"""

def get_rigid_transform(X, Y):
    """
    Calculates the rigid transform (translation, rotation)
    from X to Y
    """
    X_mu = X.mean(1, keepdims=True)
    Y_mu = Y.mean(1, keepdims=True)

    A = X - X_mu
    B = Y - Y_mu

    C = B.dot(A.T)
    U,S,Vh = la.svd(C)
    det = la.det(U.dot(Vh))
    R = U.dot(np.diag([1,1,det])).dot(Vh)

    t = Y_mu - R.dot(X_mu)
    return R, t



if __name__ == "__main__":
    X = np.array([
        [3,2,1],
        [0,2,1],
        [0,0,9],
        [0,0,9]])

    Y = X + 2

    X = X.T
    Y = Y.T

    R, t = get_rigid_transform(X,Y)

    transformed = R.T.dot(X) + d