2023-09-07 14:19:18 +03:00
|
|
|
|
from typing import Tuple
|
|
|
|
|
|
|
2023-09-05 15:54:19 +03:00
|
|
|
|
import numpy
|
|
|
|
|
|
|
2023-09-07 14:19:18 +03:00
|
|
|
|
from numpy.linalg import svd
|
|
|
|
|
|
|
2023-09-05 15:54:19 +03:00
|
|
|
|
|
|
|
|
|
|
def centroid(points: numpy.ndarray) -> numpy.ndarray:
|
|
|
|
|
|
return points.mean(axis=0)
|
|
|
|
|
|
|
|
|
|
|
|
|
2023-09-07 14:19:18 +03:00
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
|
|
# Вычисляем матрицу кросс-ковариаций
|
|
|
|
|
|
cross_covariance_matrix = numpy.dot(numpy.transpose(centred_actual_points), centred_expected_points)
|
|
|
|
|
|
|
|
|
|
|
|
# Применяем к полученной матрице кросс-вариаций сингулярное разложение
|
|
|
|
|
|
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)
|
2023-09-05 15:54:19 +03:00
|
|
|
|
|
|
|
|
|
|
|
2023-09-07 14:19:18 +03:00
|
|
|
|
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
|
