Start fixing algorithms

pycar/src/car/tracking/algorithms.py

@@ -109,32 +109,37 @@ def calc_groups(scan):
         List of groups that were found.
     """
     prevPoint = None
-    currentGroup = None
-    allGroups = []
+    currentGroup = Group(0)
+    allGroups = [currentGroup]
     currentGroupNumber = 0
-
+    num_iters = 0
     # assume the list is already sorted.
     for point in scan:
+        num_iters += 1
         if prevPoint is None:
             prevPoint = point
+            currentGroup.add_point(point)
             continue
 
         # Distances are in mm.
-        # within 1cm makes a group. Will need to play around with this.
-        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
-            if currentGroup is None:
-                currentGroup = Group(currentGroupNumber)
-                allGroups.append(currentGroup)
+        # within 10cm makes a group. Will need to play around with this.
+        if (point[2] - prevPoint[2]) ** 2 < 100 ** 2:
             currentGroup.add_point(point)
         else:
-            if currentGroup is not None:
-                currentGroupNumber += 1
-                currentGroup = None
+            currentGroupNumber += 1
+            currentGroup = Group(currentGroupNumber)
+            currentGroup.add_point(point)
+            allGroups.append(currentGroup)
 
         prevPoint = point
+    print(num_iters)
+    print(len(allGroups))
+    for group in allGroups:
+        print(len(group.get_points()))
 
     return allGroups
 
+
 def calc_groups_edge_algorithm(scan):
     """
     Calculates groups using an edge algorithm. This takes advantage of numpy arrays
@@ -144,9 +149,11 @@ def calc_groups_edge_algorithm(scan):
     allGroups = []
     scanArray = np.array(scan)
 
+
 def edge_algorithm():
     pass
 
+
 def find_centre(group):
     """
     Gets a tuple (x,y) of the centre of the group.
@@ -168,13 +175,25 @@ def assign_groups(prev_groups, new_groups):
     """
     Assigns group numbers to a new scan based on the groups of an old scan.
     """
+    max_group_number = 0
+    unassigned_groups = []
     for group in prev_groups:
         old_centre = find_centre(group)
         for new_group in new_groups:
             new_centre = find_centre(new_group)
-            # They are considered the same if the new group and old group centres are within 5cm.
-            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
+            # They are considered the same if the new group and old group centres are within 10cm.
+            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 100 ** 2:
                 new_group.number = group.number
+                if group.number > max_group_number:
+                    max_group_number = group.number
+                continue
+        # 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