Start adding icp algorithms

pycar/src/car/tracking/icp.py

@@ -0,0 +1,71 @@
+"""
+A module that includes algorithms to execute the Iterative Closest Point (ICP) algorithm in Besl (1992)
+
+This algorithm reshapes (registers) a set of points to match those in another data set. In object tracking,
+this basically moves the tracked groups to be in line with the correct position in the new scan. Additionally, it
+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]])
+        min_distance = -1
+        # Could vectorise this, but the speed advantage would be minimal
+        for point in model_vector:
+            euclid_d = euclid_distance(data, point)
+            if min_distance < 0 or euclid_d < min_distance:
+                current_closest = point
+                min_distance = euclid_d
+        return current_closest
+
+    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):
+    """
+    Calculates the mean squared error for the points, after a registration.
+    """
+    pass
+
+def calc_cross_cov(data: np.ndarray, model: np.ndarray) -> np.ndarray:
+    """
+    Calculates the cross covariance matrix of the two point clouds. 
+    """
+    return (data.dot(model.T).sum(0) / data.shape[0]) - calc_mass_centre(data).dot(calc_mass_centre(model).T)
+
+def calc_quaternion(cross_cov):
+    """
+    Calculates the optimal unit quaternion (the optimal rotation for this iteration3,3)
+    """
+    pass
+
+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
+
+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])]], 
+    []])
+    
+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)