From ec677890c45012495f1d488b981b8c673c1e5031 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Sun, 14 Jun 2020 19:48:13 +0930
Subject: [PATCH] Start adding updated assign groups algorithm

---
 pycar/src/car/tracking/algorithms.py | 14 +++++++++++++-
 pycar/src/car/tracking/animate.py    |  2 +-
 pycar/src/car/tracking/icp.py        |  3 +++
 3 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/pycar/src/car/tracking/algorithms.py b/pycar/src/car/tracking/algorithms.py
index 036712e..68446dc 100644
--- a/pycar/src/car/tracking/algorithms.py
+++ b/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):
     """
diff --git a/pycar/src/car/tracking/animate.py b/pycar/src/car/tracking/animate.py
index 3e31974..0066137 100755
--- a/pycar/src/car/tracking/animate.py
+++ b/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
diff --git a/pycar/src/car/tracking/icp.py b/pycar/src/car/tracking/icp.py
index 1c454dd..384e042 100644
--- a/pycar/src/car/tracking/icp.py
+++ b/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