| 20 | |
| 21 | |
| 22 | class LShapeFitting(): |
| 23 | |
| 24 | class Criteria(Enum): |
| 25 | AREA = 1 |
| 26 | CLOSENESS = 2 |
| 27 | VARIANCE = 3 |
| 28 | |
| 29 | def __init__(self): |
| 30 | # Parameters |
| 31 | self.criteria = self.Criteria.VARIANCE |
| 32 | self.min_dist_of_closeness_crit = 0.01 # [m] |
| 33 | self.dtheta_deg_for_serarch = 1.0 # [deg] |
| 34 | self.R0 = 3.0 # [m] range segmentation param |
| 35 | self.Rd = 0.001 # [m] range segmentation param |
| 36 | |
| 37 | def fitting(self, ox, oy): |
| 38 | |
| 39 | # step1: Adaptive Range Segmentation |
| 40 | idsets = self._adoptive_range_segmentation(ox, oy) |
| 41 | |
| 42 | # step2 Rectangle search |
| 43 | rects = [] |
| 44 | for ids in idsets: # for each cluster |
| 45 | cx = [ox[i] for i in range(len(ox)) if i in ids] |
| 46 | cy = [oy[i] for i in range(len(oy)) if i in ids] |
| 47 | rects.append(self._rectangle_search(cx, cy)) |
| 48 | |
| 49 | return rects, idsets |
| 50 | |
| 51 | def _calc_area_criterion(self, c1, c2): |
| 52 | c1_max = max(c1) |
| 53 | c2_max = max(c2) |
| 54 | c1_min = min(c1) |
| 55 | c2_min = min(c2) |
| 56 | |
| 57 | alpha = -(c1_max - c1_min) * (c2_max - c2_min) |
| 58 | |
| 59 | return alpha |
| 60 | |
| 61 | def _calc_closeness_criterion(self, c1, c2): |
| 62 | c1_max = max(c1) |
| 63 | c2_max = max(c2) |
| 64 | c1_min = min(c1) |
| 65 | c2_min = min(c2) |
| 66 | |
| 67 | D1 = [min([np.linalg.norm(c1_max - ic1), |
| 68 | np.linalg.norm(ic1 - c1_min)]) for ic1 in c1] |
| 69 | D2 = [min([np.linalg.norm(c2_max - ic2), |
| 70 | np.linalg.norm(ic2 - c2_min)]) for ic2 in c2] |
| 71 | |
| 72 | beta = 0 |
| 73 | for i, _ in enumerate(D1): |
| 74 | d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_crit) |
| 75 | beta += (1.0 / d) |
| 76 | |
| 77 | return beta |
| 78 | |
| 79 | def _calc_variance_criterion(self, c1, c2): |