Optimal rotation in 3d with Kabsch algorithm

user_11981188 0
Viewed 2025

The Problem

I am trying to use the Kabsch algorithm to find the optimal rotation between two sets of 4 points, but the errors I am getting are unacceptably large. I want to map the red points (P) into the blue points (Q), shown here.

What I've tried

Here is my implementation of the Kabsch algorithm, given numpy arrays of the mapping_points (P) and true_points (Q)

mapped_centroid = np.average(mapping_points, axis=0)
true_centroid = np.average(true_points, axis=0)

mapping_points -= mapped_centroid
true_points -= true_centroid

h = np.matmul(mapping_points.T, true_points)
u, s, v = np.linalg.svd(h)

d = np.linalg.det(v @ u.T)
e = np.array([[1, 0, 0], [0, 1, 0], [0, 0, d]])

r = v @ e @ u.T
tt = true_centroid - np.matmul(r, mapped_centroid)

I then apply the mapping to the centroid-adjusted points in P:

map_list = []
for i in mapping_points:
    point = np.matmul(r, i) + tt
    map_list.append(np.reshape(point, (1, 3)))
mapped_xyz = np.vstack(map_list)

However, plotting the mapped_xyz and true_points gives very inaccurate results shown in the above plot. Any ideas on what I've done wrong?

Additional information

I am actually trying to map between 2 networks of several hundred nodes. One network is the "true positions", the other is a "relative map" of the network. I am using the anchors nodes Q and the corresponding points P to find the best transformation, and then map the rest of the nodes.

I have noticed that there is a strong sensitivity to the choice of anchor nodes (which I am sampling at random). With only 3 anchor nodes, the mapping is occasionally close, sometimes off by a single rotation, sometimes mirrored about the z-axis. I have been banging my head against the wall trying to figure this out. Any help is greatly appreciated.

Edit

Here is the data for the points Q and P used in the above plot.

Q = [[ 1774.11606309 -4241.11341178  5259.04277742]
     [ 6079.70499031   -98.14197972 -3442.0914569 ]
     [  813.07069876  3334.26289147 -6112.55652513]
     [ 1856.72080823  2328.86927901  6322.16611888]]
P = [[ 3172.79468418   727.52462347  7122.70450243]
     [  165.28953155 -3552.32467068 -2045.15346584]
     [ 5292.45250241 -1748.52037006 -6181.40300009]
     [ 1893.07584225  5897.19719625  3130.41287776]]
math algorithm python
1 Answers

I finally figured it out (kicking myself for it). numpy's linalg.svd returns the transpose of v (technically conjugate transpose), so my kabsch implementation was incorrect. This explains why it was getting wildly different results depending on the anchor node selection. It should be:

h = mapping_points.T @ true_points
u, s, vt = np.linalg.svd(h)
v = vt.T

d = np.linalg.det(v @ u.T)
e = np.array([[1, 0, 0], [0, 1, 0], [0, 0, d]])

r = v @ e @ u.T
user_119811880
Related