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

34
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,39 +46,45 @@ 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)
"""
Q = np.array([[[cross_cov.trace()], [delta.T]],
[[delta], [cross_cov + cross_cov.T - cross_cov.trace().dot(np.identity(3))]]])
Q = np.array([[[cross_cov.trace()], [delta.T]],
[[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]]])
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]]])
def calc_mass_centre(points: np.ndarray) -> np.ndarray:
"""
Calculates the point at the centre of mass for the given set of points.
The returned vector is in form 1xn, where point set is of shape mxn
"""
return points.sum(0) / len(points)
return points.sum(0) / len(points)