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)