From b7c35336d3ddecfd0d38b6b6c605b535b75a6b90 Mon Sep 17 00:00:00 2001
From: Pavel Naboka <Naboka@diakont.com>
Date: Mon, 11 Sep 2023 16:26:22 +0300
Subject: [PATCH] =?UTF-8?q?dev:=20=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?=
 =?UTF-8?q?=D0=B5=D0=BD=20=D1=81=D0=BA=D1=80=D0=B8=D0=BF=D1=82=20points=5F?=
 =?UTF-8?q?generator.py,=20=D0=BA=D0=BE=D1=82=D0=BE=D1=80=D1=8B=D0=B9=20?=
 =?UTF-8?q?=D0=B3=D0=B5=D0=BD=D0=B5=D1=80=D0=B8=D1=80=D1=83=D0=B5=D1=82=20?=
 =?UTF-8?q?=D0=B8=D1=81=D1=85=D0=BE=D0=B4=D0=BD=D1=8B=D0=B5=20=D1=84=D0=B0?=
 =?UTF-8?q?=D0=B9=D0=BB=D1=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Дополнительно был проведён некоторый рефактор основных скриптов
---
 .gitignore          |  2 ++
 main.py             | 16 ++++++++++++----
 points_generator.py | 35 +++++++++++++++++++++++++++++++++++
 transformations.py  |  5 ++++-
 4 files changed, 53 insertions(+), 5 deletions(-)
 create mode 100644 points_generator.py

diff --git a/.gitignore b/.gitignore
index c04bc49..216aa7d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
 poetry.lock
+/expected_points.txt
+/actual_points.txt
diff --git a/main.py b/main.py
index 4c3f9b7..c8a4369 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, calculate_rotation_matrix, convert_rotation_matrix_to_angles
+from transformations import calculate_rotation_matrix, centroid, convert_rotation_matrix_to_angles
 
 expected_points_file_path = Path('expected_points.txt')
 actual_points_file_path = Path('actual_points.txt')
@@ -21,6 +21,11 @@ if __name__ == '__main__':
     expected_points = read_points_from_file(expected_points_file_path)
     actual_points = read_points_from_file(actual_points_file_path)
 
+    plot_3d.plot(expected_points[:, 0], expected_points[:, 1], expected_points[:, 2],
+                 'o-', markersize=12, linewidth=3, label='Expected Points')
+    plot_3d.plot(actual_points[:, 0], actual_points[:, 1], actual_points[:, 2],
+                 'o-', markersize=12, linewidth=3, label='Actual Points')
+
     rotation_matrix = calculate_rotation_matrix(actual_points, expected_points)
     angles = convert_rotation_matrix_to_angles(rotation_matrix)
 
@@ -30,10 +35,13 @@ if __name__ == '__main__':
     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)
+    print(f"offset = {tuple(offset)}")
+
+    plot_3d.plot(actual_points_converted[:, 0], actual_points_converted[:, 1], actual_points_converted[:, 2],
+                 'o-', markersize=12, linewidth=3, label='Transformed actual points')
+
     plot_3d.grid(True)
     plot_3d.tick_params(labelsize=15)
 
+    pyplot.legend()
     pyplot.show()
diff --git a/points_generator.py b/points_generator.py
new file mode 100644
index 0000000..3dc05d4
--- /dev/null
+++ b/points_generator.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+from pathlib import Path
+
+import numpy as np
+
+
+def rotation_matrix(sigma: float) -> np.ndarray:
+
+    radians = sigma * np.pi / 180.0
+
+    r11 = np.cos(radians)
+    r12 = -np.sin(radians)
+    r21 = np.sin(radians)
+    r22 = np.cos(radians)
+
+    R = np.array([[r11, r12, 0], [r21, r22, 0], [0, 0, 1]])
+
+    return R
+
+
+A = np.array([[1.0, 1.0, 0.0], [1.0, 2.0, 1.0], [2.0, 1.5, 2.0], [0.0, 0.5, 3.0]])
+
+with Path('expected_points.txt').open('w') as actual_points_output_file:
+    for line in A:
+        actual_points_output_file.write(' '.join((str(number) for number in line)) + '\n')
+
+B = A.copy()
+B *= 1.4
+B -= 3
+B = np.dot(B, rotation_matrix(90))
+
+with Path('actual_points.txt').open('w') as actual_points_output_file:
+    for line in B:
+        actual_points_output_file.write(' '.join((str(number) for number in line)) + '\n')
diff --git a/transformations.py b/transformations.py
index 1e6c7b6..33fe885 100644
--- a/transformations.py
+++ b/transformations.py
@@ -13,9 +13,11 @@ def calculate_rotation_matrix(actual_points: numpy.ndarray,
                               expected_points: numpy.ndarray) -> numpy.ndarray:
     """
     Вычисление матрицы поворота, который необходим для приближения текущих точек к ожидаемым, на основе алгоритма Кабша
+
     :param actual_points: массив текущих точек
     :param expected_points: массив ожидаемых точек
     :return: Матрица поворотов
+
     """
     # Для применения алгоритма Кабша необходимо,
     # чтобы центроиды обрабатываемых массивов точек совпадали с началом координат
@@ -39,10 +41,11 @@ def calculate_rotation_matrix(actual_points: numpy.ndarray,
 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)