【草稿】L-Shape fitting

1.rectangle_fitting.py

# Object shape recognition with L-shape fitting

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

from simulator import VehicleSimulator, LidarSimulator

show_animation = True


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):

        # Adaptive Range Segmentation进行自适应分割
        idsets = self._adoptive_range_segmentation(ox, oy)
        # output：[{0}, {1, 2, 3, 4, 5, 6, 7}, {8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18}]

        # 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)  # 最大的x'
        c2_max = max(c2)  # 最大的y'
        c1_min = min(c1)  # 最小的x'
        c2_min = min(c2)  # 最小的y'

        # 分别取c1、c2中的方差最小值作为D1、D2
        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  # 表示矩阵乘运算 新的x'下行是新的y'
            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)

        # calculate 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):  # 为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中删除掉另一个
                    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
        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]

    def calc_cross_point(self, 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
    simtime = 30.0  # simulation time模拟时间
    dt = 0.2  # time tick模拟间隔
    angle_reso = np.deg2rad(3.0)  # sensor angle resolution传感器角分辨率
    # 模拟两辆车V1和V2（ix, iy, iyaw, iv, max_v, w, L）
    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

        v1.update(dt, 0.1, 0.0)  # 时间、加速度大小、加速度方向变化率
        v2.update(dt, 0.1, -0.05)

        ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso)  # 获取观测数据点

        rects, idsets = lshapefitting.fitting(ox, oy)

        if show_animation:  # pragma: no cover
            plt.cla()
            plt.axis("equal")
            plt.plot(0.0, 0.0, "*r")
            v1.plot()
            v2.plot()

            # Plot range observation
            for ids in idsets:
                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()

2.simulator

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):
        # Positions Vehicle is expected to occupy
        self.x = ix  # 中心点为x,y
        self.y = iy
        self.yaw = iyaw  # 偏角
        self.v = iv  # 速度
        self.max_v = max_v  # 最大速度限制
        self.W = w  # 车辆宽
        self.L = L  # 车辆长
        self._calc_vehicle_contour()  # 插值后的车辆轮廓点对vc_x,vc_y

    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")

    # Vehicle Pose Estimation加入车辆的姿态的具体坐标
    def calc_global_contour(self):  # zip将对应列表元素打包为元组的列表
        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):  # 插值,插值完成后出现100对点
        rx, ry = [], []
        dtheta = 0.05
        for i in range(len(x) - 1):  # range()取前不取后
            # 每一段之间都插入20对点对
            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 = [], [], [], []

        # storing 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)  # x、y、弧度、半径（母线）
                y.append(vy)
                angle.append(vangle)
                r.append(vr)

        # ray casting filter以弧度和距离返回在扫描线上的点的x,y坐标
        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("Begin!!")
    print("Fitting Done!!")


if __name__ == '__main__':
    main()