139 lines
3.6 KiB
Python
139 lines
3.6 KiB
Python
import math
|
|
|
|
|
|
class Group:
|
|
|
|
def __init__(self, number, points=[]):
|
|
self._points = 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 in 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 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
|
|
-------
|
|
list
|
|
List of groups that were found.
|
|
"""
|
|
prevPoint = None
|
|
currentGroup = None
|
|
allGroups = []
|
|
currentGroupNumber = 0
|
|
|
|
# assume the list is already sorted.
|
|
for point in scan:
|
|
if prevPoint is None:
|
|
prevPoint = point
|
|
continue
|
|
|
|
# Distances are in mm.
|
|
# within 1cm makes a group. Will need to play around with this.
|
|
if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
|
|
if currentGroup is None:
|
|
currentGroup = Group(currentGroupNumber)
|
|
allGroups.append(currentGroup)
|
|
currentGroup.add_point(point)
|
|
else:
|
|
if currentGroup is not None:
|
|
currentGroupNumber += 1
|
|
currentGroup = None
|
|
|
|
prevPoint = point
|
|
|
|
return allGroups
|
|
|
|
|
|
def find_centre(group):
|
|
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.
|
|
"""
|
|
for group in prev_groups:
|
|
old_centre = find_centre(prev_groups)
|
|
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 5cm.
|
|
if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
|
|
new_group.set_number(group.get_number())
|
|
|
|
return new_groups
|
|
|
|
|
|
def updateCarVelocity(oldGroup, newGroup):
|
|
"""
|
|
Return a vector indicating how the tracked group has changed, which can
|
|
be used to then update the steering/throttle of the car (or other vehicle that
|
|
may be used)
|
|
"""
|
|
pass
|