python - 使用 Kabsch 算法进行 3d 最佳旋转
问题描述
问题
我正在尝试使用Kabsch 算法来找到两组 4 点之间的最佳旋转,但我得到的错误大得无法接受。我想将红点 ( P ) 映射到蓝点 ( Q ),如图所示。
我试过的
这是我对 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 个网络之间进行映射。一个网络是“真实位置”,另一个是网络的“相对地图”。我正在使用锚节点Q和相应的点P来找到最佳转换,然后映射其余节点。
我注意到对锚节点的选择有很强的敏感性(我是随机抽样的)。只有 3 个锚节点,映射偶尔会很接近,有时会被单次旋转关闭,有时会围绕 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]]
解决方案
我终于想通了(为此踢自己)。numpy 的 linalg.svd 返回 v 的转置(技术上共轭转置),所以我的 kabsch 实现不正确。这就解释了为什么它会根据锚节点的选择而得到截然不同的结果。它应该是:
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
推荐阅读
- mysql - MySql 显示不同的结果以与 SqlLite 进行比较
- angular - 在 for 循环中等待几个 observables 的结果
- java - bundles-osgi.wiring.package=com.mashape.unirest.http (version>=2.0.0)(!(version>=3.0.0)) 中未解决的约束
- ios - AVAssetExportSession 无法在 iOS13 设备上同时编译 .mp4 视频和 .aac 音频
- jenkins - 无法从 Bitbucket 触发管道 Jenkins 构建
- linux - 每次监视执行时都有不同的文件间接寻址
- json - 如何用 jq 命令替换 JSON 值?
- angular - 在反应式表单上使用正则表达式进行 Angular 7 电子邮件验证
- python-3.x - 从其他功能更改条目小部件值
- amazon-ec2 - Rancher 卡在等待 SSH 可用