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
+    currentGroup = Group(0)
-    allGroups = []
+    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.
+        # within 10cm makes a group. Will need to play around with this.
-        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
+        if (point[2] - prevPoint[2]) ** 2 < 100 ** 2:
-            if currentGroup is None:
-                currentGroup = Group(currentGroupNumber)
-                allGroups.append(currentGroup)
             currentGroup.add_point(point)
         else:
-            if currentGroup is not None:
             currentGroupNumber += 1
-                currentGroup = None
+            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.
+            # 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) < 50 ** 2:
+            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
 

pycar/src/car/tracking/lidar_cache.py

@@ -5,6 +5,7 @@ import zmq
 from car.tracking.devices.mock_lidar import MockLidar
 import car.tracking.lidar_loader as lidar_loader
 import time
+import timeit
 
 
 class LidarCache():
@@ -49,6 +50,7 @@ class LidarCache():
             if not self.run:
                 break
 
+            start_time = time.time()
             # Now process the groups.
             if self.currentGroups is not None:
                 self.currentGroups = algorithms.assign_groups(
@@ -56,6 +58,7 @@ class LidarCache():
             else:
                 self.currentGroups = algorithms.calc_groups(scan)
 
+            print("total time: " + (str)(time.time() - start_time))
             self._fireGroupsChanged()
 
     def _fireGroupsChanged(self):
@@ -99,9 +102,7 @@ class LidarCache():
 
 if __name__ == '__main__':
     lidar = MockLidar(iter(lidar_loader.load_scans_bytes_file(
-        'car/src/car/tracking/out.pickle')))
+        'pycar/src/car/tracking/out.pickle')))
     cache = LidarCache(lidar)
-    cache.add_groups_changed_listener(lambda a: print(a))
+    # cache.add_groups_changed_listener(lambda a: print(a))
     cache.start_cache()
-    time.sleep(1)
-    cache.stop_scanning()