Least-Squares Fitting of Two 3-D Point Sets

icp.py

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from transforms3d import euler

fig = plt.figure()
ax = fig.gca(projection='3d')

p1 = np.zeros((3, 100))
p1[0, :] = np.linspace(1, 3, 100)
p1[1, :] = np.sin(p1[0, :])
p1[2, :] = np.cos(p1[0, :])

R = euler.euler2mat(10, 20, 30)
T = np.array([0.1, 0.2, 0.3]).reshape((-1, 1))

p2 = R.dot(p1) + T

p1c = np.mean(p1, axis=1).reshape((-1, 1))
p2c = np.mean(p2, axis=1).reshape((-1, 1))

q1 = p1 - p1c
q2 = p2 - p2c

H = sum([q1[:, i].reshape(-1, 1).dot(q2[:, i].reshape(1, -1))
         for i in range(q1.shape[1])])
print('H:\n', H)

U, _, V = np.linalg.svd(H)
R2 = V.T.dot(U.T)
print('det ok:', np.allclose(np.linalg.det(R2), 1.0))

print('R ok:', np.allclose(R, R2))
T2 = p2c - R.dot(p1c)
print('T ok:', np.allclose(T, T2))

ax.scatter(p1[0, :], p1[1, :], p1[2, :], label='p1', c='b')
ax.scatter(p2[0, :], p2[1, :], p2[2, :], label='p2', c='r')

p3 = R2.dot(p1) + T2
ax.scatter(p3[0, :], p3[1, :], p3[2, :], label='p3', c='g')

ax.legend()
plt.show()