More work on new assign groups algorithm.

pycar/src/car/tracking/algorithms.py

@@ -2,6 +2,7 @@ import math
 import numpy as np
 from . import icp
 
+
 class Group:
 
     def __init__(self, number):
@@ -105,8 +106,8 @@ def calc_groups(scan):
 
     Returns
     -------
-    list
-        List of groups that were found.
+    ndarray
+        Array of groups that were found.
     """
     prevPoint = None
     currentGroup = Group(0)
@@ -130,7 +131,7 @@ def calc_groups(scan):
             allGroups.append(currentGroup)
 
         prevPoint = point
-    return allGroups
+    return np.array(allGroups)
 
 
 def calc_groups_edge_algorithm(scan):
@@ -183,13 +184,13 @@ def assign_groups(prev_groups, new_groups):
         # If this is reached, then no matching groups were found.
         unassigned_groups.append(new_group)
 
-
     for group in unassigned_groups:
         max_group_number += 1
         group.number = max_group_number
 
     return new_groups
 
+
 def assign_groups_II(prev_groups, new_groups):
     """
     Performs the assign groups algorithm, but instead of being greedy to assign, it will match up the
@@ -197,10 +198,30 @@ def assign_groups_II(prev_groups, new_groups):
 
     Additionally, the centre of mass for a group of points is now used, which is less prone to the effects of 
     outliers as the existing find_centre algorithm.
+
+    An ICP rotation/translation is not made in this algorithm, as it's assumed that the scans are quick enough for
+    there to not be a significant difference between scans that would require ICP. 
     """
     max_group_number = 0
     unassigned_groups = []
-    new_group_centers = numpy.array()
+
+    def centres_from_groups(group):
+        return icp.calc_mass_centre(np.array(group.points))
+
+    centre_func = np.vectorize(centres_from_groups)
+    old_group_centres = centre_func(prev_groups)
+    old_group_indexes = np.arange(len(old_group_centres))
+
+    new_group_centers = centre_func(new_groups)
+    new_group_indexes = np.arange(len(new_group_centers))
+
+    closest_points = icp.closest_points(new_group_centers, old_group_centres)
+    # Now assign the new groups to the closest matching old group, if the distance is within a certain threshold.
+    for i, point in enumerate(closest_points):
+        matching_groups = prev_groups[old_group_centres == point]
+        new_groups[i].number = prev_groups[0].number
+
+    return new_groups
 
 
 def updateCarVelocity(oldGroup, newGroup):