问题
我正在尝试使用卡布什算法找到两组 4 点之间的最佳旋转,但我得到的错误大得令人无法接受。我想绘制红点(P)到蓝点(Q),如图所示here.
我尝试过的
这是我对 Kabsch 算法的实现,给定 numpy 数组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)
然后,我将映射应用于中的质心调整点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
and true_points
给出上图所示的非常不准确的结果。关于我做错了什么有什么想法吗?
附加信息
我实际上正在尝试在具有数百个节点的两个网络之间进行映射。一个网络是“真实位置”,另一个网络是网络的“相对地图”。我正在使用锚节点Q以及对应的点P找到最佳变换,然后映射其余节点。
我注意到锚节点的选择(我随机采样)有很强的敏感性。由于只有 3 个锚节点,映射有时会很接近,有时会因单次旋转而偏离,有时会绕 z 轴镜像。我一直用头撞墙试图弄清楚这一点。任何帮助是极大的赞赏。
Edit
这是点的数据Q and 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]]