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

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

@@ -9,6 +9,7 @@ can provide an estimation as to how the tracked groups have moved.
import math import math
import numpy as np import numpy as np
def closest_points(data_shape, model_shape): def closest_points(data_shape, model_shape):
""" """
Returns the closest points from data to model in the same order as the data shape. Returns the closest points from data to model in the same order as the data shape.
@@ -27,14 +28,17 @@ def closest_points(data_shape, model_shape):
closest_func = np.vectorize(calc_closest_point) closest_func = np.vectorize(calc_closest_point)
return closest_func(data_shape, model_shape) return closest_func(data_shape, model_shape)
def euclid_distance(point1, point2): def euclid_distance(point1, point2):
return math.sqrt(((point1[0] - point2[0]) ** 2) + ((point1[1] - point2[1]) ** 2)) 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. 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: 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) 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): def calc_delta(cross_cov):
A = (cross_cov - cross_cov.T) A = (cross_cov - cross_cov.T)
return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]]) return np.ndarray([[A[2][3]], [A[3][1]], [A[1][2]]])
def calc_quaternion(cross_cov, delta): def calc_quaternion(cross_cov, delta):
""" """
Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3) Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3)
@@ -56,21 +62,25 @@ def calc_quaternion(cross_cov, delta):
optimal_eigen_index = eigen_values[eigen_values == eigen_values.max()][0] 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): 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 Calculates the optimal translation with the given (optimal) quaternion (which is converted to a rotation
matrix within the funtion) matrix within the funtion)
""" """
pass return mass_centre_model - optimal_rotation.dot(mass_centre_data)
def calc_rotation_matrix(q): def calc_rotation_matrix(q):
""" """
Returns the rotation matrix for the given unit quaternion. 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])]], 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[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[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: def calc_mass_centre(points: np.ndarray) -> np.ndarray:
""" """
Calculates the point at the centre of mass for the given set of points. Calculates the point at the centre of mass for the given set of points.