From 0edb6e02fd0cd2f20d802dcef1fd4870a75638f8 Mon Sep 17 00:00:00 2001 From: Pavel Naboka Date: Thu, 14 Sep 2023 17:41:53 +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=D0=B0=20=D0=B2=D0=BE=D0=B7=D0=BC=D0=BE=D0=B6=D0=BD?= =?UTF-8?q?=D0=BE=D1=81=D1=82=D1=8C=20=D1=80=D0=B0=D1=81=D1=87=D1=91=D1=82?= =?UTF-8?q?=D0=B0=20=D0=BC=D0=B0=D1=82=D1=80=D0=B8=D1=86=D1=8B=20=D0=BF?= =?UTF-8?q?=D0=BE=D0=B2=D0=BE=D1=80=D0=BE=D1=82=D0=B0=20=D0=B8=20=D1=81?= =?UTF-8?q?=D0=BC=D0=B5=D1=89=D0=B5=D0=BD=D0=B8=D1=8F=20=D0=BF=D0=BE=20?= =?UTF-8?q?=D0=BE=D0=B3=D1=80=D0=B0=D0=BD=D0=B8=D1=87=D0=B5=D0=BD=D0=BD?= =?UTF-8?q?=D0=BE=D0=BC=D1=83=20=D0=BA=D0=BE=D0=BB=D0=B8=D1=87=D0=B5=D1=81?= =?UTF-8?q?=D1=82=D0=B2=D1=83=20=D1=82=D0=BE=D1=87=D0=B5=D0=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main.py | 58 ++++++++++++++++++++++++++++++++++++---------- transformations.py | 18 ++++++++++---- 2 files changed, 60 insertions(+), 16 deletions(-) diff --git a/main.py b/main.py index c8a4369..8203aa2 100644 --- a/main.py +++ b/main.py @@ -2,43 +2,77 @@ from pathlib import Path import numpy from matplotlib import pyplot +from matplotlib.figure import figaspect, SubplotParams +from mpl_toolkits.mplot3d import Axes3D 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') -plot_3d = pyplot.figure().add_subplot(projection='3d') +plot_3d: Axes3D = pyplot.figure(subplotpars=SubplotParams(0, 0, 1, 1, 0, 0), + figsize=(10, 8)).add_subplot(projection='3d') def read_points_from_file(points_file_path: Path) -> numpy.ndarray: + """ + Чтение пути из файла + + :param points_file_path: путь к файлу + :return: Путь, вычитанный из файла + + """ with points_file_path.open('r') as points_file: return numpy.array([[float(coordinate) for coordinate in point_line.strip().split(' ')] for point_line in points_file]) +def rsmd(first_path: numpy.ndarray, second_path: numpy.ndarray) -> float: + """ + Расчёт среднеквадратичное отклонение между путями + + :param first_path: первый путь + :param second_path: второй путь + :return: СКО + + """ + diff = first_path - second_path + return numpy.sqrt((diff * diff).sum() / first_path.shape[0]) + + 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') + 's-k', 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') + 's-g', markersize=12, linewidth=3, + label='Actual Points') - rotation_matrix = calculate_rotation_matrix(actual_points, expected_points) - angles = convert_rotation_matrix_to_angles(rotation_matrix) + for calculation_points_number in range(len(actual_points), 1, -1): + print(f"{15 * '='} Calc on {calculation_points_number:2} points {15 * '='}") + rotation_matrix = calculate_rotation_matrix(actual_points, + expected_points, + calculation_points_number=calculation_points_number) + angles = convert_rotation_matrix_to_angles(rotation_matrix) - actual_points_rotated = numpy.dot(actual_points, rotation_matrix) + actual_points_rotated = numpy.dot(actual_points, rotation_matrix) - offset = centroid(actual_points_rotated) - centroid(expected_points) - actual_points_converted = actual_points_rotated - offset + actual_points_for_offset = actual_points_rotated[:calculation_points_number] + expected_points_for_offset = expected_points[:calculation_points_number] - print(f"{angles = }") - print(f"offset = {tuple(offset)}") + offset = centroid(actual_points_for_offset) - centroid(expected_points_for_offset) + actual_points_converted = actual_points_rotated - 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') + print(f" {angles = }") + print(f" offset = {tuple(offset)}") + print(f" error = {rsmd(expected_points, actual_points_converted)}") + + plot_3d.plot(actual_points_converted[:, 0], actual_points_converted[:, 1], actual_points_converted[:, 2], + 'o-', markersize=9, linewidth=3, + label=f'Transformed actual points (on {calculation_points_number} points)') plot_3d.grid(True) plot_3d.tick_params(labelsize=15) diff --git a/transformations.py b/transformations.py index 33fe885..cc2af9d 100644 --- a/transformations.py +++ b/transformations.py @@ -1,4 +1,4 @@ -from typing import Tuple +from typing import Optional, Tuple import numpy @@ -10,19 +10,29 @@ def centroid(points: numpy.ndarray) -> numpy.ndarray: def calculate_rotation_matrix(actual_points: numpy.ndarray, - expected_points: numpy.ndarray) -> numpy.ndarray: + expected_points: numpy.ndarray, + *, + calculation_points_number: Optional[int] = None) -> numpy.ndarray: """ Вычисление матрицы поворота, который необходим для приближения текущих точек к ожидаемым, на основе алгоритма Кабша :param actual_points: массив текущих точек :param expected_points: массив ожидаемых точек + :param calculation_points_number: число используемых для поворота точек :return: Матрица поворотов """ + sliced_actual_points = actual_points[:calculation_points_number] \ + if isinstance(calculation_points_number, int) \ + else actual_points + sliced_expected_points = expected_points[:calculation_points_number] \ + if isinstance(calculation_points_number, int) \ + else expected_points + # Для применения алгоритма Кабша необходимо, # чтобы центроиды обрабатываемых массивов точек совпадали с началом координат - centred_actual_points = actual_points - centroid(actual_points) - centred_expected_points = expected_points - centroid(expected_points) + centred_actual_points = sliced_actual_points - centroid(sliced_actual_points) + centred_expected_points = sliced_expected_points - centroid(sliced_expected_points) # Вычисляем матрицу кросс-ковариаций cross_covariance_matrix = numpy.dot(numpy.transpose(centred_actual_points), centred_expected_points)