18 lines
542 B
Python
18 lines
542 B
Python
|
import numpy
|
||
|
|
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
if (numpy.linalg.det(V) * numpy.linalg.det(W)) < 0.0:
|
||
|
|
S[-1] = -S[-1]
|
||
|
|
V[:, -1] = -V[:, -1]
|
||
|
|
|
||
|
|
return numpy.dot(V, W)
|