35 lines
1.2 KiB
Python
35 lines
1.2 KiB
Python
|
from pathlib import Path
|
||
|
|
|
||
|
|
import numpy
|
||
|
|
from matplotlib import pyplot
|
||
|
|
|
||
|
|
from transformations import centroid, kabsch_algorithm
|
||
|
|
|
||
|
|
expected_points_file_path = Path('expected_points.txt')
|
||
|
|
actual_points_file_path = Path('actual_points.txt')
|
||
|
|
|
||
|
|
plot_3d = pyplot.figure().add_subplot(projection='3d')
|
||
|
|
|
||
|
|
|
||
|
|
def read_points_from_file(points_file_path: Path) -> numpy.ndarray:
|
||
|
|
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])
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
expected_points = read_points_from_file(expected_points_file_path)
|
||
|
|
actual_points = read_points_from_file(actual_points_file_path)
|
||
|
|
|
||
|
|
expected_points -= centroid(expected_points)
|
||
|
|
actual_points -= centroid(actual_points)
|
||
|
|
|
||
|
|
actual_points = numpy.dot(actual_points, kabsch_algorithm(actual_points, expected_points))
|
||
|
|
|
||
|
|
plot_3d.plot(actual_points[:, 0], actual_points[:, 1], actual_points[:, 2], 'o-', markersize=12, linewidth=3)
|
||
|
|
plot_3d.plot(expected_points[:, 0], expected_points[:, 1], expected_points[:, 2], 'o-', markersize=12, linewidth=3)
|
||
|
|
plot_3d.grid(True)
|
||
|
|
plot_3d.tick_params(labelsize=15)
|
||
|
|
|
||
|
|
pyplot.show()
|