Nabokka/PathPointsTransformer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dev: Добавлена возможность расчёта матрицы поворота и смещения по ограниченному количеству точек

Browse Source
This commit is contained in:
Павел Набока 2023-09-14 17:41:53 +03:00
parent b7c35336d3
commit 0edb6e02fd
2 changed files with 60 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
main.py
View File

@ -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)

18
transformations.py
View File

@ -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)