diff --git a/src/components/detection/l_shape_fitting/l_shape_fitting_detector.py b/src/components/detection/l_shape_fitting/l_shape_fitting_detector.py index fabf16da..25d055b6 100644 --- a/src/components/detection/l_shape_fitting/l_shape_fitting_detector.py +++ b/src/components/detection/l_shape_fitting/l_shape_fitting_detector.py @@ -2,6 +2,11 @@ l_shape_fitting_detector.py Author: Shisato Yano + +Reference: + X. Zhang, W. Xu, C. Dong and J. M. Dolan, + "Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners", + 2017 IEEE Intelligent Vehicles Symposium (IV), pp. 54-59, 2017. """ import sys @@ -21,6 +26,22 @@ class LShapeFittingDetector: """ Object detection class by L shape fitting algorithm + + A vehicle seen by a 2D laser scanner looks like an "L": the scanner only + sees the one or two sides of the car that face it. This class turns a + point cloud into one bounding rectangle per object in three steps: + + 1. Segment: group points that are close to each other into small + clusters (_adaptive_range_segmentation) and merge clusters that share + points into one cluster per object (_merge_clusters). + 2. Search the direction: for each cluster, try many rectangle directions + theta and keep the one that scores best on the "variance criterion" + (_calculate_variance_criterion). + 3. Build the rectangle: take the minimum and maximum of the points along + the two axes of that best direction (_calculate_rectangle). + + The direction search and the variance criterion follow Zhang et al. (2017), + see the reference at the top of this file. """ def __init__(self, min_rng_th_m=3.0, rng_th_rate=0.1, change_angle_deg=1.0): @@ -39,6 +60,15 @@ def __init__(self, min_rng_th_m=3.0, rng_th_rate=0.1, change_angle_deg=1.0): def _adaptive_range_segmentation(self, point_cloud): """ Private function to segment each points based on adaptive range + + Two points belong to the same small cluster when their distance is + below a threshold that grows with the distance of the point from the + sensor, because a laser scanner samples far objects more sparsely: + + r_th = MIN_RNG_TH_M + RNG_TH_RATE * d + + where d[m] is the distance from the sensor to the point being checked. + point_cloud: Input original point cloud Return list of small clusters """ @@ -69,36 +99,53 @@ def _adaptive_range_segmentation(self, point_cloud): return clusters_list - def _mearge_clusters(self, cluster_list): + def _merge_clusters(self, cluster_list): """ Private function to merge small clusters into same large cluster + + Neighborhoods found around different points overlap. Whenever two + clusters have at least one point in common they describe the same + object, so they are replaced by their union. This repeats until no + two clusters share a point. + cluster_list: Small clusters list Return list of merged large clusters """ - mearged_list = copy.deepcopy(cluster_list) + merged_list = copy.deepcopy(cluster_list) while True: changed_list = False # permutations of 2 clusters combination - index_perm_list = list(itertools.permutations(range(len(mearged_list)), 2)) + index_perm_list = list(itertools.permutations(range(len(merged_list)), 2)) for (index_1, index_2) in index_perm_list: - if mearged_list[index_1] & mearged_list[index_2]: - mearged_list[index_1] = (mearged_list[index_1] | mearged_list.pop(index_2)) + if merged_list[index_1] & merged_list[index_2]: + merged_list[index_1] = (merged_list[index_1] | merged_list.pop(index_2)) changed_list = True break if not changed_list: break - return mearged_list + return merged_list def _rotate_points(self, points_array, angle_rad): """ Private function to rotate points - points_array: Input original points - angle_rad: Rotation angle - Return rotated points array + + Expresses every point (x, y) in a coordinate frame whose first axis + points in the direction theta (the candidate rectangle direction): + + c1 = cos(theta) * x + sin(theta) * y + c2 = -sin(theta) * x + cos(theta) * y + + In this frame the candidate rectangle is axis-aligned, so its four + edges are simply c1 = min(c1), c1 = max(c1), c2 = min(c2) and + c2 = max(c2). + + points_array: Input original points, shape (2, N) rows [x, y] + angle_rad: Rotation angle theta[rad] + Return rotated points array, shape (2, N) rows [c1, c2] """ angle_cos = cos(angle_rad) @@ -112,8 +159,33 @@ def _rotate_points(self, points_array, angle_rad): def _calculate_variance_criterion(self, points_array): """ Private function to calculate variance of distance between point and rectangle's edge - points_array: Input rotated points - Return variance distance as cost + + This is the "variance criterion" of Zhang et al. (2017). For a good + direction, the points of an L-shaped cluster lie along the rectangle's + edges, so every point is very close to one of them. + + For each point i, with the rectangle spanning [c1_min, c1_max] x + [c2_min, c2_max], the distance to the nearest edge along each axis is + + d1_i = min(c1_max - c1_i, c1_i - c1_min) + d2_i = min(c2_max - c2_i, c2_i - c2_min) + + Each point is assigned to the axis whose edge it is closest to: + + E1 = { d1_i : d1_i < d2_i } (points on an edge of axis 1) + E2 = { d2_i : d1_i >= d2_i } (points on an edge of axis 2) + + and the criterion is the negative total variance of those distances: + + gamma = -Var(E1) - Var(E2) + + If the points hug the edges, the distances are all about the same + (near zero), the variances are small, and gamma is close to 0, its + maximum. A poor direction spreads the distances out and makes gamma + more negative. So the best direction is the one with the LARGEST gamma. + + points_array: Input rotated points, shape (2, N) rows [c1, c2] + Return gamma, the negative variance of distances (larger is better) """ c1 = points_array[0, :] @@ -135,10 +207,25 @@ def _calculate_variance_criterion(self, points_array): def _calculate_rectangle(self, points_array): """ Private function to calculate the most fitted rectangle + + Tries a rectangle direction theta every CHANGE_ANGLE_RAD, rotates the + cluster by theta, scores it with the variance criterion and keeps the + theta with the largest score. A rectangle looks the same after a + quarter turn, so only directions in [0, 90 deg) need to be searched. + + With the best theta, the rectangle is the box around the rotated + points. Its four edges are lines a*x + b*y = c in the original frame: + + cos(theta)*x + sin(theta)*y = c1_min cos(theta)*x + sin(theta)*y = c1_max + -sin(theta)*x + cos(theta)*y = c2_min -sin(theta)*x + cos(theta)*y = c2_max + + which are the coefficient lists a, b, c given to Rectangle below. + points_array: Input points in a cluster Return calculated rectangle object """ + # (best criterion so far, its angle); the criterion is maximized, so it starts at -inf min_cost_angle = (-float("inf"), None) initial_angle_rad = 0.0 end_angle_rad = np.pi / 2.0 - self.CHANGE_ANGLE_RAD @@ -182,9 +269,9 @@ def update(self, point_cloud): clusters_list = self._adaptive_range_segmentation(point_cloud) - mearged_clusters_list = self._mearge_clusters(clusters_list) + merged_clusters_list = self._merge_clusters(clusters_list) - self._search_rectangles(mearged_clusters_list) + self._search_rectangles(merged_clusters_list) def draw(self, axes, elems, x_m, y_m, yaw_rad): """