From ecca22d7d4e503d93c7eb263cb9370fe4ad81ad2 Mon Sep 17 00:00:00 2001
From: michaelpivato <michael.pivato@powerhealthsolutions.com>
Date: Mon, 15 Jun 2020 12:29:11 +0930
Subject: [PATCH] More work on new assign groups algorithm.

---
 pycar/src/car/tracking/algorithms.py  | 31 ++++++++++++++++++++++-----
 pycar/src/car/tracking/lidar_cache.py |  2 +-
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/pycar/src/car/tracking/algorithms.py b/pycar/src/car/tracking/algorithms.py
index 68446dc..6f20cb3 100644
--- a/pycar/src/car/tracking/algorithms.py
+++ b/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):
diff --git a/pycar/src/car/tracking/lidar_cache.py b/pycar/src/car/tracking/lidar_cache.py
index 673ea66..5d1df91 100644
--- a/pycar/src/car/tracking/lidar_cache.py
+++ b/pycar/src/car/tracking/lidar_cache.py
@@ -50,7 +50,7 @@ class LidarCache():
 
             # Now process the groups.
             if self.currentGroups is not None:
-                self.currentGroups = algorithms.assign_groups(
+                self.currentGroups = algorithms.assign_groups_II(
                     self.currentGroups, algorithms.calc_groups(scan))
             else:
                 self.currentGroups = algorithms.calc_groups(scan)