From 1172647481992a669344395e3c0ae1fa05601b67 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Mon, 15 Jun 2020 20:38:14 +0930 Subject: [PATCH] Fix up bugs with assign groups 2 --- pycar/src/car/tracking/algorithms.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pycar/src/car/tracking/algorithms.py b/pycar/src/car/tracking/algorithms.py index 6f20cb3..db8f35b 100644 --- a/pycar/src/car/tracking/algorithms.py +++ b/pycar/src/car/tracking/algorithms.py @@ -205,20 +205,20 @@ def assign_groups_II(prev_groups, new_groups): max_group_number = 0 unassigned_groups = [] - def centres_from_groups(group): - return icp.calc_mass_centre(np.array(group.points)) + def centres_from_groups(groups): + return np.array([icp.calc_mass_centre(np.array([convert_lidar_to_cartesian(point) for point in group.get_points()])) for group in groups]) - centre_func = np.vectorize(centres_from_groups) - old_group_centres = centre_func(prev_groups) + old_group_centres = centres_from_groups(prev_groups) old_group_indexes = np.arange(len(old_group_centres)) - new_group_centers = centre_func(new_groups) + new_group_centers = centres_from_groups(new_groups) new_group_indexes = np.arange(len(new_group_centers)) closest_points = icp.closest_points(new_group_centers, old_group_centres) # Now assign the new groups to the closest matching old group, if the distance is within a certain threshold. for i, point in enumerate(closest_points): matching_groups = prev_groups[old_group_centres == point] + # TODO: Check the centres are within a certain threshold. new_groups[i].number = prev_groups[0].number return new_groups