Source code for Mapping.rectangle_fitting.rectangle_fitting

"""

Object shape recognition with L-shape fitting

author: Atsushi Sakai (@Atsushi_twi)

Ref:
- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners -
The Robotics Institute Carnegie Mellon University
https://www.ri.cmu.edu/publications/
efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/

"""

import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))

import matplotlib.pyplot as plt
import numpy as np
import itertools
from enum import Enum

from utils.angle import rot_mat_2d

from Mapping.rectangle_fitting.simulator \
    import VehicleSimulator, LidarSimulator

show_animation = True


[docs]class LShapeFitting: """ LShapeFitting class. You can use this class by initializing the class and changing the parameters, and then calling the fitting method. """
[docs] class Criteria(Enum): AREA = 1 CLOSENESS = 2 VARIANCE = 3
def __init__(self): """ Default parameter settings """ #: Fitting criteria parameter self.criteria = self.Criteria.VARIANCE #: Minimum distance for closeness criteria parameter [m] self.min_dist_of_closeness_criteria = 0.01 #: Angle difference parameter [deg] self.d_theta_deg_for_search = 1.0 #: Range segmentation parameter [m] self.R0 = 3.0 #: Range segmentation parameter [m] self.Rd = 0.001
[docs] def fitting(self, ox, oy): """ Fitting L-shape model to object points Parameters ---------- ox : x positions of range points from an object oy : y positions of range points from an object Returns ------- rects: Fitting rectangles id_sets: id sets of each cluster """ # step1: Adaptive Range Segmentation id_sets = self._adoptive_range_segmentation(ox, oy) # step2 Rectangle search rects = [] for ids in id_sets: # for each cluster cx = [ox[i] for i in range(len(ox)) if i in ids] cy = [oy[i] for i in range(len(oy)) if i in ids] rects.append(self._rectangle_search(cx, cy)) return rects, id_sets
@staticmethod def _calc_area_criterion(c1, c2): c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2) alpha = -(c1_max - c1_min) * (c2_max - c2_min) return alpha def _calc_closeness_criterion(self, c1, c2): c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2) # Vectorization d1 = np.minimum(c1_max - c1, c1 - c1_min) d2 = np.minimum(c2_max - c2, c2 - c2_min) d = np.maximum(np.minimum(d1, d2), self.min_dist_of_closeness_criteria) beta = (1.0 / d).sum() return beta @staticmethod def _calc_variance_criterion(c1, c2): c1_max, c1_min, c2_max, c2_min = LShapeFitting._find_min_max(c1, c2) # Vectorization d1 = np.minimum(c1_max - c1, c1 - c1_min) d2 = np.minimum(c2_max - c2, c2 - c2_min) e1 = d1[d1 < d2] e2 = d2[d1 >= d2] v1 = - np.var(e1) if len(e1) > 0 else 0. v2 = - np.var(e2) if len(e2) > 0 else 0. gamma = v1 + v2 return gamma @staticmethod def _find_min_max(c1, c2): c1_max = max(c1) c2_max = max(c2) c1_min = min(c1) c2_min = min(c2) return c1_max, c1_min, c2_max, c2_min def _rectangle_search(self, x, y): xy = np.array([x, y]).T d_theta = np.deg2rad(self.d_theta_deg_for_search) min_cost = (-float('inf'), None) for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta): c = xy @ rot_mat_2d(theta) c1 = c[:, 0] c2 = c[:, 1] # Select criteria cost = 0.0 if self.criteria == self.Criteria.AREA: cost = self._calc_area_criterion(c1, c2) elif self.criteria == self.Criteria.CLOSENESS: cost = self._calc_closeness_criterion(c1, c2) elif self.criteria == self.Criteria.VARIANCE: cost = self._calc_variance_criterion(c1, c2) if min_cost[0] < cost: min_cost = (cost, theta) # calc best rectangle sin_s = np.sin(min_cost[1]) cos_s = np.cos(min_cost[1]) c1_s = xy @ np.array([cos_s, sin_s]).T c2_s = xy @ np.array([-sin_s, cos_s]).T rect = RectangleData() rect.a[0] = cos_s rect.b[0] = sin_s rect.c[0] = min(c1_s) rect.a[1] = -sin_s rect.b[1] = cos_s rect.c[1] = min(c2_s) rect.a[2] = cos_s rect.b[2] = sin_s rect.c[2] = max(c1_s) rect.a[3] = -sin_s rect.b[3] = cos_s rect.c[3] = max(c2_s) return rect def _adoptive_range_segmentation(self, ox, oy): # Setup initial cluster segment_list = [] for i, _ in enumerate(ox): c = set() r = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]]) for j, _ in enumerate(ox): d = np.hypot(ox[i] - ox[j], oy[i] - oy[j]) if d <= r: c.add(j) segment_list.append(c) # Merge cluster while True: no_change = True for (c1, c2) in list(itertools.permutations(range(len(segment_list)), 2)): if segment_list[c1] & segment_list[c2]: segment_list[c1] = (segment_list[c1] | segment_list.pop(c2)) no_change = False break if no_change: break return segment_list
class RectangleData: def __init__(self): self.a = [None] * 4 self.b = [None] * 4 self.c = [None] * 4 self.rect_c_x = [None] * 5 self.rect_c_y = [None] * 5 def plot(self): self.calc_rect_contour() plt.plot(self.rect_c_x, self.rect_c_y, "-r") def calc_rect_contour(self): self.rect_c_x[0], self.rect_c_y[0] = self.calc_cross_point( self.a[0:2], self.b[0:2], self.c[0:2]) self.rect_c_x[1], self.rect_c_y[1] = self.calc_cross_point( self.a[1:3], self.b[1:3], self.c[1:3]) self.rect_c_x[2], self.rect_c_y[2] = self.calc_cross_point( self.a[2:4], self.b[2:4], self.c[2:4]) self.rect_c_x[3], self.rect_c_y[3] = self.calc_cross_point( [self.a[3], self.a[0]], [self.b[3], self.b[0]], [self.c[3], self.c[0]]) self.rect_c_x[4], self.rect_c_y[4] = self.rect_c_x[0], self.rect_c_y[0] @staticmethod def calc_cross_point(a, b, c): x = (b[0] * -c[1] - b[1] * -c[0]) / (a[0] * b[1] - a[1] * b[0]) y = (a[1] * -c[0] - a[0] * -c[1]) / (a[0] * b[1] - a[1] * b[0]) return x, y def main(): # simulation parameters sim_time = 30.0 # simulation time dt = 0.2 # time tick angle_resolution = np.deg2rad(3.0) # sensor angle resolution v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0), 0.0, 50.0 / 3.6, 3.0, 5.0) v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0), 0.0, 50.0 / 3.6, 4.0, 10.0) l_shape_fitting = LShapeFitting() lidar_sim = LidarSimulator() time = 0.0 while time <= sim_time: time += dt v1.update(dt, 0.1, 0.0) v2.update(dt, 0.1, -0.05) ox, oy = lidar_sim.get_observation_points([v1, v2], angle_resolution) rects, id_sets = l_shape_fitting.fitting(ox, oy) if show_animation: # pragma: no cover plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") plt.plot(0.0, 0.0, "*r") v1.plot() v2.plot() # Plot range observation for ids in id_sets: x = [ox[i] for i in range(len(ox)) if i in ids] y = [oy[i] for i in range(len(ox)) if i in ids] for (ix, iy) in zip(x, y): plt.plot([0.0, ix], [0.0, iy], "-og") plt.plot([ox[i] for i in range(len(ox)) if i in ids], [oy[i] for i in range(len(ox)) if i in ids], "o") for rect in rects: rect.plot() plt.pause(0.1) print("Done") if __name__ == '__main__': main()