DBSCAN 对点云障碍物聚类

最新推荐文章于 2024-06-08 18:29:35 发布

RNG_uzi_

最新推荐文章于 2024-06-08 18:29:35 发布

阅读量2.9k

点赞数 1

文章标签： DBSCAN 对点云障碍物聚类

原文链接：https://zhuanlan.zhihu.com/p/72121128

版权

3D点云同时被 2 个专栏收录

28 篇文章 30 订阅

订阅专栏

PCL

19 篇文章 37 订阅

订阅专栏

DBSCAN 对点云障碍物聚类

（感谢前辈）转自：https://zhuanlan.zhihu.com/p/72121128

点云数据去除地面后，地面上的点很自然的都成了障碍物，但是要进行目标分类，还需要把每个目标的一堆障碍物的点聚集到一起，然后才好进行后续的分析，因为每个点都是空间上离的很近的点，那么很自然的，就想到了可以使用基于密度的聚类 DBSCAN ；

为了更好的适应我的需求，使用的是自己写的DBSCAN方法；

整个过程如下：

1 先做降采样，降低点云数量；

2 然后地面去除，得到障碍物的点；

3 然后把剩余的地表障碍物点送入 DBSCAN 进行聚类；

4 最后进行可视化；

聚类核心代码如下：

def vector_distance_v2(v):
    """
    把单个向量内部的每个元素两两相减，得到一个差值矩阵，矩阵是上三角和下三角刚好相反的结果
    :param v: 可以是一个一维数组，或者一个一维的列表
    :return:
    """
    if type(v) is list:
        v = np.array(v)
    # result = []
    # for i in range(len(v)):
    #     result.append(v[i] - v)  # 可以改为列表推导式
    result = [v[i] - v  for i in range(len(v))]
    return np.vstack(result)   


def point_distance(points):
    """
    计算所有 points 两两之间的距离
    :param points:  地面分割之后检测出来的点  n * 4
    :return:  n * n 的距离矩阵
    """
    d2 = vector_distance_v2(points[:,0])**2 + \
         vector_distance_v2(points[:,1])**2 + \
         vector_distance_v2(points[:,2])**2


    return np.sqrt(d2)




# @profile
def DBSCAN_points(points, eps=2., Minpts=15):
    """
    基于密度的点云聚类
    :param d_bbox: 点与点之间的距离矩阵
    :param eps:  最大搜索直径阈值
    :param Minpts:  最小包含其他对象数量阈值
    :return: 返回聚类结果，是一个嵌套列表,每个子列表就是这个区域的对象的序号
    """
    # 先求距离
    print('DBSCAN clustering:',points.shape)
    d_bbox = point_distance(points)

    #初始化核心对象集合T,聚类个数k,聚类集合C, 未访问集合P,
    T = set()
    k = 0
    C = []
    P = set(range(d_bbox.shape[0]))
    # print('P',P)
    for d in range(d_bbox.shape[0]):
        # print(np.sum( d_bbox[d,:] <= eps))
        if np.sum( d_bbox[d,:] <= eps) >= Minpts:
            T.add(d)  # 最初的核心对象
    print('Len T: ',len(T))
    #开始聚类
    while len(T):
        P_old = P  #
        o = list(T)[np.random.randint(0, len(T))]  # 从T中随机选取一个核心元素
        # o = list(T)[random.randint(0, len(T)-1)]  # 从T中随机选取一个核心元素
        # print('o: ',o)
        P = P - set([o])
        Q = []
        Q.append(o)
        # print('Q: ',Q)

        while len(Q):
            q = Q[0]
            # print('q: ', q)
            # Nq = [i for i in range(d_bbox.shape[0]) if d_bbox[q,i] <= eps] #q的领域密度
            Nq = np.where(d_bbox[q,:] <= eps)[0]
            if len(Nq) >= Minpts:
                S = P & set(Nq)   # 这个核心对象的密度可达对象与未访问对象的交集
                Q += (list(S))   # 把这个核心对象以及它的密度可达对象都包含进来，对所有的对象再做多次密度可达检测
                P = P - S  # 未访问集合P 减去 这个核心对象的密度可达对象
            # print('S: ', S)
            # print('Nq: ', Nq)
            # print('P: ', P)

            Q.remove(q)  # q 已经做过密度可达检测了，去掉它
        # print('------')
        k += 1
        Ck = P_old - P # 原有的P和去掉了该核心对象的密度可达对象的P就是该类的所有对象
        T = T - Ck  # 去掉该类对象里面包含的核心对象
        C.append(Ck)     # 把该类的对象加入列表
    # print('noise points:', P)   # 最后没有被归类的数据点就是噪音点
    return C