Проблема
Я пытаюсь использовать алгоритм Кабша , чтобы найти оптимальное вращение между двумя наборами из 4 точек, но ошибки, которые я получаю, недопустимо велики. Я хочу отобразить красные точки ( P ) на синие точки ( Q ), показанные здесь .
Что я пробовал
Вот моя реализация алгоритма Кабша с учетом numpy массивов mapping_points ( P ) и 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 и true_points дает очень неточные результаты, показанные на приведенном выше графике. Есть идеи о том, что я сделал неправильно?
Дополнительная информация
На самом деле я пытаюсь сопоставить 2 сети из нескольких сотен узлов. Одна сеть - это «истинные положения», другая - «относительная карта» сети. Я использую якорные узлы Q и соответствующие точки P , чтобы найти наилучшее преобразование, а затем отобразить остальные узлы .
Я заметил, что существует сильная чувствительность к выбору узлов привязки (которые я выбираю наугад). При наличии только трех узлов привязки отображение иногда близко, иногда отключается одним поворотом, иногда отражается вокруг оси 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]]