适用于4D毫米波雷达的目标矩形框聚类

目录

一、前言

二、点云聚类分割

三、基于方位搜索L型拟合

四、评价准则之面积最小化

五、评价准则之贴合最大化

六、评价准则之方差最小化


 

3da15d2e63e047c9a39ad39831be0075.png

一、前言

对于多线束雷达可以获取目标物体更全面的面貌,在道路中前向或角雷达可能无法获取目标车矩形框但可以扫到两边或者一边,在做目标方向估计时这些信息至关重要。看到一篇文章不错的思路,拿来记录借鉴下。

整体算法:
Step1: 将距离点按照预先设定的距离阈值进行聚类划分簇
Step2:基于方位搜索+特定准则进行L型拟合,得出4边参数
2.1 矩形面积最小化
2.2 点到边靠近最大化
2.3 点到边方差最小化

二、点云聚类分割

对于所有点,在距离r一点内找到它的相邻点,将相邻点设置为未检测,对未检测点继续找到距离r内的任何一个点,把邻近点放在这个簇中直到集群不再增长。

afe7f40da3d2461b9b171fd9a93dd7cd.png

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

 

三、基于方位搜索L型拟合

80db7156348e427c8728232643489652.png

dec3357dea4545cc8dba368502134009.png

step2:基于方位搜索的L 型拟合

遍历矩形的所有可能方向;在每次迭代中,可以找到一个该方向并包含所有扫描点的矩形;进一步可以得到所有点到矩形四条边的距离,并根据这些距离将点分为P和Q,并计算相应的平方误差作为(1)中的目标函数;在迭代所有方向并获得所有相应的平方误差后,寻找实现最小平方误差的最优方向,并根据该方向拟合矩形

22c4e7b4cc9e40dcacb79cd1621fee38.png

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

 

四、评价准则之面积最小化

a1cfde486c4943778a0e82a655695eb8.png

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

 

五、评价准则之贴合最大化

f211f7c1262c4bf58064bb454ff0e3c4.png

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

六、评价准则之方差最小化

0d70e1efa8b144a7b8e136647ddb8ed3.png

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

 

Result:

1fb1bca4ecd647febd4ef8a1b9b25d69.png

 

 

 

参考:《Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners》

 

 

 

 

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值