vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix up bugs with assign groups 2

Browse Source
This commit is contained in:
Piv
2020-06-15 20:38:14 +09:30
parent ecca22d7d4
commit 1172647481
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
pycar/src/car/tracking/algorithms.py
View File

@@ -205,20 +205,20 @@ def assign_groups_II(prev_groups, new_groups):
max_group_number = 0 max_group_number = 0
unassigned_groups = [] unassigned_groups = []
def centres_from_groups(group): def centres_from_groups(groups):
return icp.calc_mass_centre(np.array(group.points)) 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 = centres_from_groups(prev_groups)
old_group_centres = centre_func(prev_groups)
old_group_indexes = np.arange(len(old_group_centres)) 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)) new_group_indexes = np.arange(len(new_group_centers))
closest_points = icp.closest_points(new_group_centers, old_group_centres) 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. # 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): for i, point in enumerate(closest_points):
matching_groups = prev_groups[old_group_centres == point] 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 new_groups[i].number = prev_groups[0].number
return new_groups return new_groups