python - 使用 Kabsch 算法在 3d 中进行最佳旋转

Question

问题

我正在尝试使用 Kabsch algorithm 来找到两组 4 点之间的最佳旋转，但我得到的错误大得无法接受。我想将红点( P )映射到蓝点( Q )，如图 here 。

我试过的

这是我对 Kabsch 算法的实现，给定 mapping_points ( P ) 和 true_points ( Q ) 的 numpy 数组

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)

然后我将映射应用于 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)

但是，绘制 mapped_xyz 和 true_points 会给出上图中显示的非常不准确的结果。关于我做错了什么的任何想法？

附加信息

我实际上是在尝试在包含数百个节点的 2 个网络之间进行映射。一个网络是“真实位置”，另一个是网络的“相对 map ”。我使用 anchor 节点 Q 和对应点 P 来找到最佳变换，然后映射其余节点。

我注意到对 anchor 节点的选择(我随机抽样)有很强的敏感性。只有 3 个 anchor 节点，映射有时接近，有时通过单次旋转关闭，有时围绕 z 轴镜像。我一直在用头撞墙试图弄清楚这一点。任何帮助是极大的赞赏。

编辑

这是上图中使用的点 Q 和 P 的数据。

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]]

Answer 1

我终于想通了(为此而踢自己)。 numpy 的 linalg.svd 返回 v 的转置(技术上共轭转置)，所以我的 kabsch 实现是不正确的。这解释了为什么根据 anchor 节点选择会得到截然不同的结果。它应该是:

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