Rigid registration of two point clouds with known correspondence

user_3994399 0
Viewed 1613

Imagine I have two (python) lists (with a limited) amount of 3D points. How do I find a rigid transformation to match the points as closely as possible. For each point of each list it is known to which other point that point corresponds. Is there an algorithm/library for this?

I found the Iterative closest point algorithm, but this assumes there is no correspondence known and it seems to be made for large point clouds. I'm talking about limited sets of 3 to 8 points.

Possible point sets (correspondence according to index in the list)

a = [[0,0,0], [1,0,0],[0,1,0]]
b = [[0,0,0.5], [1,0,1],[0,0,2]]
python
1 Answers

Turns out there is actually an analytical solution. It is described in the paper of Arun et al., 1987, Least square fitting of two 3D point sets

I wrote a testscript to test their algorithm and it seems to work fine (if you want to have a solution that minimizes the sum of the square errors, if you have an outlier this might not be ideal):

import numpy as np

##Based on Arun et al., 1987

#Writing points with rows as the coordinates
p1_t = np.array([[0,0,0], [1,0,0],[0,1,0]])
p2_t = np.array([[0,0,1], [1,0,1],[0,0,2]]) #Approx transformation is 90 degree rot over x-axis and +1 in Z axis

#Take transpose as columns should be the points
p1 = p1_t.transpose()
p2 = p2_t.transpose()

#Calculate centroids
p1_c = np.mean(p1, axis = 1).reshape((-1,1)) #If you don't put reshape then the outcome is 1D with no rows/colums and is interpeted as rowvector in next minus operation, while it should be a column vector
p2_c = np.mean(p2, axis = 1).reshape((-1,1))

#Subtract centroids
q1 = p1-p1_c
q2 = p2-p2_c

#Calculate covariance matrix
H=np.matmul(q1,q2.transpose())

#Calculate singular value decomposition (SVD)
U, X, V_t = np.linalg.svd(H) #the SVD of linalg gives you Vt

#Calculate rotation matrix
R = np.matmul(V_t.transpose(),U.transpose())

assert np.allclose(np.linalg.det(R), 1.0), "Rotation matrix of N-point registration not 1, see paper Arun et al."

#Calculate translation matrix
T = p2_c - np.matmul(R,p1_c)

#Check result
result = T + np.matmul(R,p1)
if np.allclose(result,p2):
    print("transformation is correct!")
else:
    print("transformation is wrong...")
user_39943990
Related