Last active
February 5, 2024 11:04
-
-
Save nh2/bc4e2981b0e213fefd4aaa33edfb3893 to your computer and use it in GitHub Desktop.
Rigidly (+scale) aligns two point clouds with know point-to-point correspondences, in Python with numpy
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
import numpy as np | |
import numpy.linalg | |
# Relevant links: | |
# - http://stackoverflow.com/a/32244818/263061 (solution with scale) | |
# - "Least-Squares Rigid Motion Using SVD" (no scale but easy proofs and explains how weights could be added) | |
# Rigidly (+scale) aligns two point clouds with know point-to-point correspondences | |
# with least-squares error. | |
# Returns (scale factor c, rotation matrix R, translation vector t) such that | |
# Q = P*cR + t | |
# if they align perfectly, or such that | |
# SUM over point i ( | P_i*cR + t - Q_i |^2 ) | |
# is minimised if they don't align perfectly. | |
def umeyama(P, Q): | |
assert P.shape == Q.shape | |
n, dim = P.shape | |
centeredP = P - P.mean(axis=0) | |
centeredQ = Q - Q.mean(axis=0) | |
C = np.dot(np.transpose(centeredP), centeredQ) / n | |
V, S, W = np.linalg.svd(C) | |
d = (np.linalg.det(V) * np.linalg.det(W)) < 0.0 | |
if d: | |
S[-1] = -S[-1] | |
V[:, -1] = -V[:, -1] | |
R = np.dot(V, W) | |
varP = np.var(a1, axis=0).sum() | |
c = 1/varP * np.sum(S) # scale factor | |
t = Q.mean(axis=0) - P.mean(axis=0).dot(c*R) | |
return c, R, t | |
# Testing | |
np.set_printoptions(precision=3) | |
a1 = np.array([ | |
[0, 0, -1], | |
[0, 0, 0], | |
[0, 0, 1], | |
[0, 1, 0], | |
[1, 0, 0], | |
]) | |
a2 = np.array([ | |
[0, 0, 1], | |
[0, 0, 0], | |
[0, 0, -1], | |
[0, 1, 0], | |
[-1, 0, 0], | |
]) | |
a2 *= 2 # for testing the scale calculation | |
a2 += 3 # for testing the translation calculation | |
c, R, t = umeyama(a1, a2) | |
print "R =\n", R | |
print "c =", c | |
print "t =\n", t | |
print "Check: a1*cR + t = a2 is", np.allclose(a1.dot(c*R) + t, a2) | |
err = ((a1.dot(c * R) + t - a2) ** 2).sum() | |
print "Residual error", err |
Thanks for your sharing. It seems that the code at line 34 varP = np.var(a1, axis=0).sum()
should be np.var(P, axis=0).sum()
.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
How would you invert the points? The alignment is working perfectly for me, thanks so much for sharing this!
Edit: Just reversed the input to
umeyama
and figured it out. Thanks again for sharing.