2dcc0d8bca
I'll merge this stuff in tracking for now, as there's some other stuff I'm working on and want these latest changes in.
272 lines
8.1 KiB
Python
272 lines
8.1 KiB
Python
import math
|
|
import numpy as np
|
|
from . import icp
|
|
|
|
|
|
class Group:
|
|
|
|
def __init__(self, number):
|
|
self._points = []
|
|
self._number = number
|
|
self._minX = None
|
|
self._maxX = None
|
|
self._minY = None
|
|
self._maxY = None
|
|
|
|
def add_point(self, point):
|
|
self._points.append(point)
|
|
self._update_min_max(point)
|
|
|
|
def get_points(self):
|
|
return self._points
|
|
|
|
@property
|
|
def number(self):
|
|
return self._number
|
|
|
|
@number.setter
|
|
def number(self, number):
|
|
self._number = number
|
|
|
|
def _update_min_max(self, new_point):
|
|
"""
|
|
Updates the min and max points for this group.
|
|
This is to determine when assigning groups whether the
|
|
same group is selected.
|
|
"""
|
|
converted_point = convert_lidar_to_cartesian(new_point)
|
|
|
|
if self._minX is None or self._minX > converted_point[0]:
|
|
self._minX = converted_point[0]
|
|
|
|
if self._maxX is None or self._maxX < converted_point[0]:
|
|
self._maxX = converted_point[0]
|
|
|
|
if self._minY is None or self._minY > converted_point[1]:
|
|
self._minY = converted_point[1]
|
|
|
|
if self._maxY is None or self._maxY < converted_point[1]:
|
|
self._maxY = converted_point[1]
|
|
|
|
def get_minX(self):
|
|
return self._minY
|
|
|
|
def get_maxX(self):
|
|
return self._maxY
|
|
|
|
def get_minY(self):
|
|
return self._minY
|
|
|
|
def get_maxY(self):
|
|
return self._maxY
|
|
|
|
|
|
def convert_lidar_to_cartesian(new_point):
|
|
x = new_point[2] * math.sin(new_point[1])
|
|
y = new_point[2] * math.cos(new_point[1])
|
|
return (x, y)
|
|
|
|
|
|
def convert_cartesian_to_lidar(x, y):
|
|
"""
|
|
Converts a point on the grid (with car as the origin) to a lidar tuple (distance, angle)
|
|
|
|
Parameters
|
|
----------
|
|
x
|
|
Horizontal component of point to convert.
|
|
|
|
y
|
|
Vertical component of point to convert.
|
|
|
|
Returns
|
|
-------
|
|
converted
|
|
A tuple (distance, angle) that represents the point. Angle is in degrees.
|
|
"""
|
|
# Angle depends on x/y position.
|
|
# if x is positive and y is positive, then angle = tan-1(y/x)
|
|
# if x is positive and y is negative, then angle = 360 + tan-1(y/x)
|
|
# if x is negative and y is positive, then angle = 180 + tan-1(y/x)
|
|
# if x is negative and y is negative, then angle = 180 + tan-1(y/x)
|
|
return (math.sqrt(x ** 2 + y ** 2), math.degrees(math.atan(y/x)) + (180 if x < 0 else 270 if y < 0 else 0))
|
|
|
|
|
|
def calc_groups(scan):
|
|
"""
|
|
Calculates groups of points from a lidar scan. The scan should
|
|
already be sorted.
|
|
|
|
Parameters
|
|
----------
|
|
|
|
scan: Iterable
|
|
The lidar scan data to get groups of.
|
|
Should be of format: (quality, angle, distance)
|
|
|
|
Returns
|
|
-------
|
|
ndarray
|
|
Array of groups that were found.
|
|
"""
|
|
prevPoint = None
|
|
currentGroup = Group(0)
|
|
allGroups = [currentGroup]
|
|
currentGroupNumber = 0
|
|
# assume the list is already sorted.
|
|
for point in scan:
|
|
if prevPoint is None:
|
|
prevPoint = point
|
|
currentGroup.add_point(point)
|
|
continue
|
|
|
|
# Distances are in mm.
|
|
# within 10cm makes a group. Will need to play around with this.
|
|
if (point[2] - prevPoint[2]) ** 2 < 100 ** 2:
|
|
currentGroup.add_point(point)
|
|
else:
|
|
currentGroupNumber += 1
|
|
currentGroup = Group(currentGroupNumber)
|
|
currentGroup.add_point(point)
|
|
allGroups.append(currentGroup)
|
|
|
|
prevPoint = point
|
|
return np.array(allGroups)
|
|
|
|
|
|
def calc_groups_edge_algorithm(scan):
|
|
"""
|
|
Calculates groups using an edge algorithm. This takes advantage of numpy arrays
|
|
and vectorisation, rather than the primitive python loop grouping, resulting in
|
|
faster grouping speeds.
|
|
"""
|
|
allGroups = []
|
|
scanArray = np.array(scan)
|
|
|
|
|
|
def edge_algorithm():
|
|
pass
|
|
|
|
|
|
def find_centre(group):
|
|
"""
|
|
Gets a tuple (x,y) of the centre of the group.
|
|
|
|
Parameters
|
|
----------
|
|
group: Group
|
|
A group of points to find the centre of.
|
|
|
|
Returns
|
|
-------
|
|
tuple (x,y)
|
|
The centre in the form of a tuple (x,y)
|
|
"""
|
|
return ((group.get_maxX() + group.get_minX()) / 2, (group.get_maxY() + group.get_minY()) / 2)
|
|
|
|
|
|
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 10cm.
|
|
if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 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
|
|
|
|
|
|
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.
|
|
|
|
An ICP rotation/translation is not made in this algorithm, as it's assumed that the scans are quick enough for
|
|
there to not be a significant difference between scans that would require ICP.
|
|
"""
|
|
max_group_number = 0
|
|
unassigned_groups = []
|
|
|
|
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])
|
|
|
|
old_group_centres = centres_from_groups(prev_groups)
|
|
old_group_indexes = np.arange(len(old_group_centres))
|
|
|
|
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
|
|
|
|
# TODO: Go through to put all groups into one (if multiple groups get same number) to avoid splits.
|
|
|
|
return new_groups
|
|
|
|
|
|
def updateCarVelocity(oldGroup, newGroup):
|
|
"""
|
|
Return a tuple (DistanceChange, AngleChange) indicating how the tracked groups have changed, which can
|
|
be used to then update the steering/throttle of the car (or other vehicle that
|
|
may be used)
|
|
|
|
Parameters
|
|
----------
|
|
oldGroup: Group
|
|
The positioning of points for the group in the last scan.
|
|
|
|
newGroup: Group
|
|
The positioning of points for the group in the latest scan.
|
|
|
|
Returns
|
|
-------
|
|
tuple (DistanceChange, AngleChange)
|
|
A tuple containing how the groups' centres changed in the form (distance,angle)
|
|
"""
|
|
old_polar = convert_cartesian_to_lidar(*find_centre(oldGroup))
|
|
new_centre = convert_cartesian_to_lidar(*find_centre(newGroup))
|
|
return (new_centre[0] - old_polar[0], new_centre[1] - old_polar[1])
|
|
|
|
|
|
def dualServoChange(newCentre, changeTuple):
|
|
"""
|
|
Gets a tuple (throttleChange, steeringChange) indicating the change that should be applied to the current
|
|
throttle/steering of an rc car that uses dual servos.
|
|
|
|
Parameters
|
|
---------
|
|
newCentre
|
|
Tuple (distance, angle) of the new centre of the tracked group.
|
|
|
|
changeTuple
|
|
Tuple (distanceChange, angleChange) from the old centre to the new centre.
|
|
|
|
Returns
|
|
-------
|
|
tuple
|
|
Tuple of (throttleChange, steeringChange) to apply to the 2 servos.
|
|
"""
|
|
return ((changeTuple[0] / 3) - (newCentre[0] / 4) + 1, 0)
|