first release

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -1,21 +1,184 @@
 """
 
-Object shape recognition with rectangle fitting
+Object shape recognition with L-shape fitting
 
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
 import matplotlib.pyplot as plt
-import math
 import numpy as np
 import itertools
-import random
+from enum import Enum
+
+from simulator import VehicleSimulator, LidarSimulator
 
 show_animation = True
 
 
-class Rectangle():
+class LShapeFitting():
+
+    class Criteria(Enum):
+        AREA = 1
+        CLOSENESS = 2
+        VARIANCE = 3
+
+    def __init__(self):
+        # Parameters
+        self.criteria = self.Criteria.VARIANCE
+        self.min_dist_of_closeness_crit = 0.01  # [m]
+        self.dtheta_deg_for_serarch = 1.0  # [deg]
+        self.R0 = 3.0  # [m] range segmentation param
+        self.Rd = 0.001  # [m] range segmentation param
+
+    def fitting(self, ox, oy):
+
+        # step1: Adaptive Range Segmentation
+        idsets = self._adoptive_range_segmentation(ox, oy)
+
+        # step2 Rectangle search
+        rects = []
+        for ids in idsets:  # 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, idsets
+
+    def _calc_area_criterion(self, c1, c2):
+        c1_max = max(c1)
+        c2_max = max(c2)
+        c1_min = min(c1)
+        c2_min = min(c2)
+
+        alpha = - (c1_max - c1_min) * (c2_max - c2_min)
+
+        return alpha
+
+    def _calc_closeness_criterion(self, c1, c2):
+        c1_max = max(c1)
+        c2_max = max(c2)
+        c1_min = min(c1)
+        c2_min = min(c2)
+
+        D1 = [min([np.linalg.norm(c1_max - ic1),
+                   np.linalg.norm(ic1 - c1_min)]) for ic1 in c1]
+        D2 = [min([np.linalg.norm(c2_max - ic2),
+                   np.linalg.norm(ic2 - c2_min)]) for ic2 in c2]
+
+        beta = 0
+        for i, _ in enumerate(D1):
+            d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_crit)
+            beta += (1.0 / d)
+
+        return beta
+
+    def _calc_variance_criterion(self, c1, c2):
+        c1_max = max(c1)
+        c2_max = max(c2)
+        c1_min = min(c1)
+        c2_min = min(c2)
+
+        D1 = [min([np.linalg.norm(c1_max - ic1),
+                   np.linalg.norm(ic1 - c1_min)]) for ic1 in c1]
+        D2 = [min([np.linalg.norm(c2_max - ic2),
+                   np.linalg.norm(ic2 - c2_min)]) for ic2 in c2]
+
+        E1, E2 = [], []
+        for (d1, d2) in zip(D1, D2):
+            if d1 < d2:
+                E1.append(d1)
+            else:
+                E2.append(d2)
+
+        V1 = 0.0
+        if E1:
+            V1 = - np.var(E1)
+
+        V2 = 0.0
+        if E2:
+            V2 = - np.var(E2)
+
+        gamma = V1 + V2
+
+        return gamma
+
+    def _rectangle_search(self, x, y):
+
+        X = np.array([x, y]).T
+
+        dtheta = np.deg2rad(self.dtheta_deg_for_serarch)
+        minp = (-float('inf'), None)
+        for theta in np.arange(0.0, np.pi / 2.0 - dtheta, dtheta):
+
+            e1 = np.array([np.cos(theta), np.sin(theta)])
+            e2 = np.array([-np.sin(theta), np.cos(theta)])
+
+            c1 = X @ e1.T
+            c2 = X @ e2.T
+
+            # Select criteria
+            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 minp[0] < cost:
+                minp = (cost, theta)
+
+        # calc best rectangle
+        sin_s = np.sin(minp[1])
+        cos_s = np.cos(minp[1])
+
+        c1_s = X @ np.array([cos_s, sin_s]).T
+        c2_s = X @ 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
+        S = []
+        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.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2)
+                if d <= R:
+                    C.add(j)
+            S.append(C)
+
+        # Merge cluster
+        while 1:
+            no_change = True
+            for (c1, c2) in list(itertools.permutations(range(len(S)), 2)):
+                if S[c1] & S[c2]:
+                    S[c1] = (S[c1] | S.pop(c2))
+                    no_change = False
+                    break
+            if no_change:
+                break
+
+        return S
+
+
+class RectangleData():
 
     def __init__(self):
         self.a = [None] * 4
@@ -47,243 +210,22 @@ class Rectangle():
         return x, y
 
 
-class VehicleSimulator():
-
-    def __init__(self, ix, iy, iyaw, iv, max_v, w, L):
-        self.x = ix
-        self.y = iy
-        self.yaw = iyaw
-        self.v = iv
-        self.max_v = max_v
-        self.W = w
-        self.L = L
-        self._calc_vehicle_contour()
-
-    def update(self, dt, a, omega):
-        self.x += self.v * math.cos(self.yaw) * dt
-        self.y += self.v * math.sin(self.yaw) * dt
-        self.yaw += omega * dt
-        self.v += a * dt
-        if self.v >= self.max_v:
-            self.v = self.max_v
-
-    def plot(self):
-        plt.plot(self.x, self.y, ".b")
-
-        # convert global coordinate
-        gx, gy = self.calc_global_contour()
-        plt.plot(gx, gy, "--b")
-
-    def calc_global_contour(self):
-        gx = [(ix * math.cos(self.yaw) + iy * math.sin(self.yaw))
-              + self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
-        gy = [(ix * math.sin(self.yaw) - iy * math.cos(self.yaw))
-              + self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
-
-        return gx, gy
-
-    def _calc_vehicle_contour(self):
-
-        self.vc_x = []
-        self.vc_y = []
-
-        self.vc_x.append(self.L / 2.0)
-        self.vc_y.append(self.W / 2.0)
-
-        self.vc_x.append(self.L / 2.0)
-        self.vc_y.append(-self.W / 2.0)
-
-        self.vc_x.append(-self.L / 2.0)
-        self.vc_y.append(-self.W / 2.0)
-
-        self.vc_x.append(-self.L / 2.0)
-        self.vc_y.append(self.W / 2.0)
-
-        self.vc_x.append(self.L / 2.0)
-        self.vc_y.append(self.W / 2.0)
-
-        self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y)
-
-    def _interporate(self, x, y):
-        rx, ry = [], []
-        dtheta = 0.05
-        for i in range(len(x) - 1):
-            rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1]
-                       for θ in np.arange(0.0, 1.0, dtheta)])
-            ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1]
-                       for θ in np.arange(0.0, 1.0, dtheta)])
-
-        rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1]
-                   for θ in np.arange(0.0, 1.0, dtheta)])
-        ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1]
-                   for θ in np.arange(0.0, 1.0, dtheta)])
-
-        return rx, ry
-
-
-def get_observation_points(vlist, angle_reso):
-    x, y, angle, r = [], [], [], []
-
-    # store all points
-    for v in vlist:
-
-        gx, gy = v.calc_global_contour()
-
-        for vx, vy in zip(gx, gy):
-            vangle = math.atan2(vy, vx)
-            vr = math.hypot(vx, vy) * random.uniform(0.99, 1.01)
-
-            x.append(vx)
-            y.append(vy)
-            angle.append(vangle)
-            r.append(vr)
-
-    # ray casting filter
-    rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
-
-    return rx, ry
-
-
-def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
-    rx, ry = [], []
-    rangedb = [float("inf") for _ in range(
-        int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
-
-    for i in range(len(thetal)):
-        angleid = int(round(thetal[i] / angle_reso))
-
-        if rangedb[angleid] > rangel[i]:
-            rangedb[angleid] = rangel[i]
-
-    for i in range(len(rangedb)):
-        t = i * angle_reso
-        if rangedb[i] != float("inf"):
-            rx.append(rangedb[i] * math.cos(t))
-            ry.append(rangedb[i] * math.sin(t))
-
-    return rx, ry
-
-
-def calc_area_criterion(c1, c2):
-    c1_max = max(c1)
-    c2_max = max(c2)
-    c1_min = min(c1)
-    c2_min = min(c2)
-
-    alpha = - (c1_max - c1_min) * (c2_max - c2_min)
-
-    return alpha
-
-
-def calc_closeness_criterion(c1, c2):
-    c1_max = max(c1)
-    c2_max = max(c2)
-    c1_min = min(c1)
-    c2_min = min(c2)
-
-    D1 = [min([np.linalg.norm(c1_max - ic1),
-               np.linalg.norm(ic1 - c1_min)]) for ic1 in c1]
-    D2 = [min([np.linalg.norm(c2_max - ic2),
-               np.linalg.norm(ic2 - c2_min)]) for ic2 in c2]
-
-    d0 = 0.01
-    beta = 0
-    for i, _ in enumerate(D1):
-        d = max(min([D1[i], D2[i]]), d0)
-        beta += (1.0 / d)
-
-    return beta
-
-
-def rectangle_search(x, y):
-
-    X = np.array([x, y]).T
-
-    dtheta = np.deg2rad(0.5)
-    minp = (-float('inf'), None)
-    for theta in np.arange(0.0, math.pi / 2.0 - dtheta, dtheta):
-
-        e1 = np.array([math.cos(theta), math.sin(theta)])
-        e2 = np.array([-math.sin(theta), math.cos(theta)])
-
-        c1 = X @ e1.T
-        c2 = X @ e2.T
-
-        # alpha = calc_area_criterion(c1, c2)
-        beta = calc_closeness_criterion(c1, c2)
-
-        # cost = alpha
-        cost = beta
-
-        if minp[0] < cost:
-            minp = (cost, theta)
-
-    # calc best rectangle
-    sin_s = math.sin(minp[1])
-    cos_s = math.cos(minp[1])
-
-    c1_s = X @ np.array([cos_s, sin_s]).T
-    c2_s = X @ np.array([-sin_s, cos_s]).T
-
-    rect = Rectangle()
-    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(ox, oy):
-
-    alpha = 0.2
-
-    # Setup initial cluster
-    S = []
-    for i, _ in enumerate(ox):
-        C = set()
-        R = alpha * np.linalg.norm([ox[i], oy[i]])
-        for j, _ in enumerate(ox):
-            d = math.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2)
-            if d <= R:
-                C.add(j)
-        S.append(C)
-
-    # Merge cluster
-    while 1:
-        no_change = True
-        for (c1, c2) in list(itertools.permutations(range(len(S)), 2)):
-            if S[c1] & S[c2]:
-                S[c1] = (S[c1] | S.pop(c2))
-                no_change = False
-                break
-        if no_change:
-            break
-
-    return S
-
-
 def main():
 
     # simulation parameters
     simtime = 30.0  # simulation time
     dt = 0.2  # time tick
 
-    angle_reso = np.deg2rad(2.0)  # sensor angle resolution
+    angle_reso = 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)
 
+    lshapefitting = LShapeFitting()
+    lidar_sim = LidarSimulator()
+
     time = 0.0
     while time <= simtime:
         time += dt
@@ -291,17 +233,9 @@ def main():
         v1.update(dt, 0.1, 0.0)
         v2.update(dt, 0.1, -0.05)
 
-        ox, oy = get_observation_points([v1, v2], angle_reso)
+        ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso)
 
-        # step1: Adaptive Range Segmentation
+        rects, idsets = lshapefitting.fitting(ox, oy)
-        idsets = adoptive_range_segmentation(ox, oy)
-
-        # step2 Rectangle search
-        rects = []
-        for ids in idsets:  # 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(rectangle_search(cx, cy))
 
         if show_animation:  # pragma: no cover
             plt.cla()
@@ -318,9 +252,9 @@ def main():
                 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],
+                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],
+                         [oy[i] for i in range(len(ox)) if i in ids],
-                # "o")
+                         "o")
             for rect in rects:
                 rect.plot()
 

Mapping/rectangle_fitting/simulator.py

@@ -0,0 +1,144 @@
+""" 
+
+Simulator
+
+author: Atsushi Sakai
+
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import random
+
+
+class VehicleSimulator():
+
+    def __init__(self, ix, iy, iyaw, iv, max_v, w, L):
+        self.x = ix
+        self.y = iy
+        self.yaw = iyaw
+        self.v = iv
+        self.max_v = max_v
+        self.W = w
+        self.L = L
+        self._calc_vehicle_contour()
+
+    def update(self, dt, a, omega):
+        self.x += self.v * np.cos(self.yaw) * dt
+        self.y += self.v * np.sin(self.yaw) * dt
+        self.yaw += omega * dt
+        self.v += a * dt
+        if self.v >= self.max_v:
+            self.v = self.max_v
+
+    def plot(self):
+        plt.plot(self.x, self.y, ".b")
+
+        # convert global coordinate
+        gx, gy = self.calc_global_contour()
+        plt.plot(gx, gy, "--b")
+
+    def calc_global_contour(self):
+        gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) +
+              self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
+        gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) +
+              self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
+
+        return gx, gy
+
+    def _calc_vehicle_contour(self):
+
+        self.vc_x = []
+        self.vc_y = []
+
+        self.vc_x.append(self.L / 2.0)
+        self.vc_y.append(self.W / 2.0)
+
+        self.vc_x.append(self.L / 2.0)
+        self.vc_y.append(-self.W / 2.0)
+
+        self.vc_x.append(-self.L / 2.0)
+        self.vc_y.append(-self.W / 2.0)
+
+        self.vc_x.append(-self.L / 2.0)
+        self.vc_y.append(self.W / 2.0)
+
+        self.vc_x.append(self.L / 2.0)
+        self.vc_y.append(self.W / 2.0)
+
+        self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y)
+
+    def _interporate(self, x, y):
+        rx, ry = [], []
+        dtheta = 0.05
+        for i in range(len(x) - 1):
+            rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1]
+                       for θ in np.arange(0.0, 1.0, dtheta)])
+            ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1]
+                       for θ in np.arange(0.0, 1.0, dtheta)])
+
+        rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1]
+                   for θ in np.arange(0.0, 1.0, dtheta)])
+        ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1]
+                   for θ in np.arange(0.0, 1.0, dtheta)])
+
+        return rx, ry
+
+
+class LidarSimulator():
+
+    def __init__(self):
+        self.range_noise = 0.01
+
+    def get_observation_points(self, vlist, angle_reso):
+        x, y, angle, r = [], [], [], []
+
+        # store all points
+        for v in vlist:
+
+            gx, gy = v.calc_global_contour()
+
+            for vx, vy in zip(gx, gy):
+                vangle = math.atan2(vy, vx)
+                vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise,
+                                                       1.0 + self.range_noise)
+
+                x.append(vx)
+                y.append(vy)
+                angle.append(vangle)
+                r.append(vr)
+
+        # ray casting filter
+        rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso)
+
+        return rx, ry
+
+    def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso):
+        rx, ry = [], []
+        rangedb = [float("inf") for _ in range(
+            int(np.floor((np.pi * 2.0) / angle_reso)) + 1)]
+
+        for i in range(len(thetal)):
+            angleid = int(round(thetal[i] / angle_reso))
+
+            if rangedb[angleid] > rangel[i]:
+                rangedb[angleid] = rangel[i]
+
+        for i in range(len(rangedb)):
+            t = i * angle_reso
+            if rangedb[i] != float("inf"):
+                rx.append(rangedb[i] * np.cos(t))
+                ry.append(rangedb[i] * np.sin(t))
+
+        return rx, ry
+
+
+def main():
+    print("start!!")
+
+    print("done!!")
+
+
+if __name__ == '__main__':
+    main()