Start adding updated assign groups algorithm

pycar/src/car/tracking/algorithms.py

@@ -1,6 +1,6 @@
 import math
 import numpy as np
-
+from . import icp
 
 class Group:
 
@@ -190,6 +190,18 @@ def assign_groups(prev_groups, new_groups):
 
     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
+    closest groups for each group.
+
+    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.
+    """
+    max_group_number = 0
+    unassigned_groups = []
+    new_group_centers = numpy.array()
+
 
 def updateCarVelocity(oldGroup, newGroup):
     """