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):
     """

pycar/src/car/tracking/animate.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 '''Animates distances and measurment quality'''
-from car.tracking.mock_lidar import MockLidar
+from car.tracking..devices.mock_lidar import MockLidar
 import matplotlib.pyplot as plt
 import numpy as np
 import matplotlib.animation as animation

pycar/src/car/tracking/icp.py

@@ -4,6 +4,9 @@ A module that includes algorithms to execute the Iterative Closest Point (ICP) a
 This algorithm reshapes (registers) a set of points to match those in another data set. In object tracking,
 this basically moves the tracked groups to be in line with the correct position in the new scan. Additionally, it
 can provide an estimation as to how the tracked groups have moved.
+
+NOTE: This module is thus far untested -> it may be used in the future, and will be tested then, but for 
+now was created to provide a deeper understanding of ICP and related algorithms.
 """
 
 import math