vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Finish ICP algorithms

Browse Source
This commit is contained in:
Piv
2020-06-13 10:21:40 +09:30
parent 3710255703
commit 1cd2e2c237
1 changed files with 22 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
pycar/src/car/tracking/icp.py
View File

@@ -9,12 +9,13 @@ can provide an estimation as to how the tracked groups have moved.
import math
import numpy as np
def closest_points(data_shape, model_shape):
"""
Returns the closest points from data to model in the same order as the data shape.
"""
def calc_closest_point(data, model_vector):
current_closest = np.array([[0,0]])
current_closest = np.array([[0, 0]])
min_distance = -1
# Could vectorise this, but the speed advantage would be minimal
for point in model_vector:
@@ -27,14 +28,17 @@ def closest_points(data_shape, model_shape):
closest_func = np.vectorize(calc_closest_point)
return closest_func(data_shape, model_shape)
def euclid_distance(point1, point2):
return math.sqrt(((point1[0] - point2[0]) ** 2) + ((point1[1] - point2[1]) ** 2))
def calc_mse(points):
def calc_mse(data, model, quaternion, translation):
"""
Calculates the mean squared error for the points, after a registration.
"""
pass
return (translation - calc_rotation_matrix(quaternion).dot(data) - translation).sum(0) / len(data)
def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
"""
@@ -42,10 +46,12 @@ def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
"""
return (data.dot(model.T).sum(0) / data.shape[0]) - calc_mass_centre(data).dot(calc_mass_centre(model).T)
def calc_delta(cross_cov):
A = (cross_cov - cross_cov.T)
return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]])
def calc_quaternion(cross_cov, delta):
"""
Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3)
@@ -54,22 +60,26 @@ def calc_quaternion(cross_cov, delta):
[[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]])
eigen_values, eigen_vectors = np.linalg.eig(Q)
optimal_eigen_index = eigen_values[eigen_values == eigen_values.max()][0]
return eigen_vectors[:,optimal_eigen_index]
return eigen_vectors[:, optimal_eigen_index]
def calc_translation(optimal_rotation, mass_centre_data, mass_centre_model):
"""
Calculates the optimal translation with the given (optimal) quaternion (which is converted to a rotation
matrix within the funtion)
"""
pass
return mass_centre_model - optimal_rotation.dot(mass_centre_data)
def calc_rotation_matrix(q):
"""
Returns the rotation matrix for the given unit quaternion.
"""
return np.array([[[q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2], [2 * (q[1] * q[2] - q[0] * q[3])], [2 * (q[1] * q[3] - q[0] * q[2])]],
[[2 * (q[1] * q[2] + q[0] * q[3])], [q[0] ** 2 + q[2] ** 2 - q[1] ** 2 - q[3] **2], [2 * (q[2] * q[3] - q[0] * q[1])]],
[[2 * (q[1] * q[3] - q[0] * q[2])], [2 * (q[2] * q[3] + q[0] * q[1])], [2 * (q[2] * q[3] + q[0] * q[1])], [q[0] ** 2 + q[3] ** 2- q[1] ** 2 - q[2] ** 2]]])
[[2 * (q[1] * q[2] + q[0] * q[3])], [q[0] ** 2 + q[2] ** 2 -
q[1] ** 2 - q[3] ** 2], [2 * (q[2] * q[3] - q[0] * q[1])]],
[[2 * (q[1] * q[3] - q[0] * q[2])], [2 * (q[2] * q[3] + q[0] * q[1])], [2 * (q[2] * q[3] + q[0] * q[1])], [q[0] ** 2 + q[3] ** 2 - q[1] ** 2 - q[2] ** 2]]])
def calc_mass_centre(points: np.ndarray) -> np.ndarray:
"""