From c3d60dc14ccaa0cc0cfd378818aac6cb0a51b684 Mon Sep 17 00:00:00 2001
From: Pavel Naboka <35613006+Zanzibarra@users.noreply.github.com>
Date: Thu, 7 Sep 2023 14:19:18 +0300
Subject: [PATCH] =?UTF-8?q?dev:=20=D0=94=D0=BE=D1=80=D0=B0=D0=B1=D0=BE?=
 =?UTF-8?q?=D1=82=D0=BA=D0=B0=20=D0=BF=D0=B5=D1=80=D0=B2=D0=BE=D0=B9=20?=
 =?UTF-8?q?=D0=B8=D1=82=D0=B5=D1=80=D0=B0=D1=86=D0=B8=D0=B8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Дополнен и отрефакторен метод реализации алгоритма Кабша, добавлен метод вычисления углов поворота
---
 main.py            | 15 ++++++++++-----
 transformations.py | 48 ++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 50 insertions(+), 13 deletions(-)

diff --git a/main.py b/main.py
index 51253f2..4c3f9b7 100644
--- a/main.py
+++ b/main.py
@@ -3,7 +3,7 @@ from pathlib import Path
 import numpy
 from matplotlib import pyplot
 
-from transformations import centroid, kabsch_algorithm
+from transformations import centroid, calculate_rotation_matrix, convert_rotation_matrix_to_angles
 
 expected_points_file_path = Path('expected_points.txt')
 actual_points_file_path = Path('actual_points.txt')
@@ -21,12 +21,17 @@ if __name__ == '__main__':
     expected_points = read_points_from_file(expected_points_file_path)
     actual_points = read_points_from_file(actual_points_file_path)
 
-    expected_points -= centroid(expected_points)
-    actual_points -= centroid(actual_points)
+    rotation_matrix = calculate_rotation_matrix(actual_points, expected_points)
+    angles = convert_rotation_matrix_to_angles(rotation_matrix)
 
-    actual_points = numpy.dot(actual_points, kabsch_algorithm(actual_points, expected_points))
+    actual_points_rotated = numpy.dot(actual_points, rotation_matrix)
 
-    plot_3d.plot(actual_points[:, 0], actual_points[:, 1], actual_points[:, 2], 'o-', markersize=12, linewidth=3)
+    offset = centroid(actual_points_rotated) - centroid(expected_points)
+    actual_points_converted = actual_points_rotated - offset
+
+    print(f"{angles = }")
+    print(f"{offset = }")
+    plot_3d.plot(actual_points_converted[:, 0], actual_points_converted[:, 1], actual_points_converted[:, 2], 'o-', markersize=12, linewidth=3)
     plot_3d.plot(expected_points[:, 0], expected_points[:, 1], expected_points[:, 2], 'o-', markersize=12, linewidth=3)
     plot_3d.grid(True)
     plot_3d.tick_params(labelsize=15)
diff --git a/transformations.py b/transformations.py
index 0dec312..1e6c7b6 100644
--- a/transformations.py
+++ b/transformations.py
@@ -1,17 +1,49 @@
+from typing import Tuple
+
 import numpy
 
+from numpy.linalg import svd
+
 
 def centroid(points: numpy.ndarray) -> numpy.ndarray:
     return points.mean(axis=0)
 
 
-def kabsch_algorithm(actual_points: numpy.ndarray, expected_points: numpy.ndarray) -> numpy.ndarray:
-    # TODO Заменить безликие C, V, S, W на что-то более внятное
-    C = numpy.dot(numpy.transpose(actual_points), expected_points)
-    V, S, W = numpy.linalg.svd(C)
+def calculate_rotation_matrix(actual_points: numpy.ndarray,
+                              expected_points: numpy.ndarray) -> numpy.ndarray:
+    """
+    Вычисление матрицы поворота, который необходим для приближения текущих точек к ожидаемым, на основе алгоритма Кабша
+    :param actual_points: массив текущих точек
+    :param expected_points: массив ожидаемых точек
+    :return: Матрица поворотов
+    """
+    # Для применения алгоритма Кабша необходимо,
+    # чтобы центроиды обрабатываемых массивов точек совпадали с началом координат
+    centred_actual_points = actual_points - centroid(actual_points)
+    centred_expected_points = expected_points - centroid(expected_points)
 
-    if (numpy.linalg.det(V) * numpy.linalg.det(W)) < 0.0:
-        S[-1] = -S[-1]
-        V[:, -1] = -V[:, -1]
+    # Вычисляем матрицу кросс-ковариаций
+    cross_covariance_matrix = numpy.dot(numpy.transpose(centred_actual_points), centred_expected_points)
 
-    return numpy.dot(V, W)
+    # Применяем к полученной матрице кросс-вариаций сингулярное разложение
+    left_singular_vectors_matrix, singular_matrix, right_singular_vectors_matrix = svd(cross_covariance_matrix)
+
+    # Некая нормализация полученного разложения (сам не знаю что здесь происходит, поэтому не трогаю)
+    if (numpy.linalg.det(left_singular_vectors_matrix) * numpy.linalg.det(right_singular_vectors_matrix)) < 0.0:
+        singular_matrix[-1] = -singular_matrix[-1]
+        left_singular_vectors_matrix[:, -1] = -left_singular_vectors_matrix[:, -1]
+
+    return numpy.dot(left_singular_vectors_matrix, right_singular_vectors_matrix)
+
+
+def convert_rotation_matrix_to_angles(rotation_matrix: numpy.ndarray) -> Tuple[float, float, float]:
+    """
+    Вычисление углов поворота модели на основе применяемой к ней матрицы поворота
+    :param rotation_matrix: матрица поворота
+    :return: Кортеж из трёх углов - угол поворота вокруг оси X, угол поворота вокруг оси Y, угол поворота вокруг оси Z
+    """
+    # TODO Требует доработки, так как иногда может всплывать деление на 0
+    alpha = numpy.arccos(rotation_matrix[2, 2] / (1 - rotation_matrix[0, 2] ** 2) ** 0.5)
+    beta = numpy.arcsin(rotation_matrix[0, 2])
+    gamma = numpy.arccos(rotation_matrix[0, 0] / (1 - rotation_matrix[0, 2] ** 2) ** 0.5)
+    return alpha, beta, gamma