[自动驾驶-目标检测] C++ PCL 连通域点云聚类

文章目录

• 引言
• 标签连通域聚类原理
• Octree + LCC 代码实现
• Octree + LCC 测试结果
• Octree + LCC 的优缺点
• 改进思路
• 参考文献

引言

在实际实现3D目标检测时，在不依靠深度学习的训练模型时，仅采用传统方法实现目标检测。而在聚类领域里面，多采用欧几里得、区域生长、Ransac等方法实现聚类。其实还有一种方法可实现聚类，也就是CloudCompare软件内的标签连通域聚类方法。这种方法的好处是耗时较短，也同样能够实现较高精度的距离聚类。
本文根据其特性做出相对应的修改，使其能够脱离CC软件插件，用C++程序以及PCL点云库实现。

标签连通域聚类原理

标签连通域算法起源来自于图像聚类，其聚类思路简单易懂。

1. 一般来说都是从图片的左上角第一个像素点开始进行搜索，首先通过搜索其相邻的像素点的灰度值(或其他判定值)来进行判定。
2. 得到搜索点的判定值之后，与设定阈值T h d i s \ Th_{dis}T h d i s ​进行比较，若满足条件，则可以视作是同一类别，则对搜索点打上类别标签，若不满足则跳过。进一步将该搜索点作为新一轮的起始点，进行递归搜索，反复打上标签，直至完全搜索不到新的像素点。
3. 继续选取未打上标签点进行新一轮的标签搜索，标记上不同于之前的标签值，按照上述步骤继续标记，最终完成标签连通域聚类。

阐述完图像的标签连通域算法的基本流程后，我们需要进一步了解在空间点云上是如何实现标签连通域算法的。

CloudCompare上的连通域算法思路：

1. 首先将输入的点云P i n p u t \ P_{input}P in p u t ​做八叉树处理。
2. 进一步将八叉树内部所有分块的中心点提取出来，重新组成新的点云数据P o c t r e e \ P_{octree}P oc t ree ​。
3. 针对P o c t r e e \ P_{octree}P oc t ree ​实现K n n \ Knn K nn搜索，即对起始点找到K \ K K个搜索点p k \ p_{k}p k ​。并获取到每个搜索点p k \ p_{k}p k ​与起始点p 0 \ p_{0}p 0 ​的距离值D k \ D_{k}D k ​。
4. 设定距离阈值D t h \ D_{th}D t h ​来进行判定，当得到D k < D t h \ D_{k}时，即可认定该搜索点与起始点的标签一致，若D k > D t h \ D_{k}>D_{th}D k ​>D t h ​，则认为该p k \ p_{k}p k ​不同于起始点标签，跳过即可。
5. 进一步通过递归迭代对未实现标记的点云重复3、4步骤，即可实现八叉树结构的标签连通域聚类。即可将点云数据P o c t r e e \ P_{octree}P oc t ree ​聚类成带m \ m m个标签的点云P l a b l e \ P_{lable}P l ab l e ​。
6. 进一步提取P l a b l e \ P_{lable}P l ab l e ​的每个点定义为新的起始点，搜索的方法采用八叉树的体素搜索方式，即只对P i n p u t \ P_{input}P in p u t ​搜索八叉树内部的其余点，并对这些搜索后的点打上中心点的相同标签。最终完成对整个P i n p u t \ P_{input}P in p u t ​的聚类任务。

其实整体看下来，距离判定处理的方式与欧几里得聚类方法相同，不同的是一个采用K n n \ Knn K nn搜索，一个为半径搜索。那么相信在讲解该算法的运行流程之后，大家会理解基于八叉树的标签连通域聚类原理。然后只需要一步步实现其代码即可。

Octree + LCC 代码实现

首先头文件。

#pragma once
#ifndef OCTREE_CONNECTION
#define OCTREE_CONNECTION

#define PCL_NO_PRECOMPILE

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include

using namespace pcl;
using namespace std;

class OTC
{

private:
    typedef pcl::PointXYZ VPoint;
    typedef std::vector<VPoint,Eigen::aligned_allocator<VPoint>> AlignedPointTVector;

    double get_points2dis(VPoint points);

    float octree_leaf_size = 0.3f;
    int max_points_size = 20000;
    int min_points_size = 2;
    int sourch_k_num = 10;
    double dis_th = 0.1;

public:

    bool octree_denoise( const pcl::PointCloud<VPoint>::Ptr cloud,
                        pcl::PointCloud<VPoint> & filter_cloud);

    void set_octree_leafsize(const float size){
        octree_leaf_size = size;
    };

    void set_min_points_size(const int min_size){
        min_points_size = min_size;
    };

    void set_distance_th(const double distance_th){
        dis_th = distance_th;
    };

    void euclidean_clusters(const PointCloud<VPoint> cloud,
                           const typename search::Search<VPoint>::Ptr &tree,
                           double tolerance, std::vector<PointIndices> &clusters,
                           unsigned int min_pts_per_cluster,
                           unsigned int max_pts_per_cluster);

    void octree_connection(const PointCloud<VPoint>::Ptr input_cloud,
                          std::vector<pcl::PointCloud<VPoint>>& octree_connect_cloud);
    OTC(){};

    ~OTC(){};

};
#endif

后续对应的cpp文件，参考欧几里得的聚类源代码实现。

void OTC::euclidean_clusters(const PointCloud<VPoint> cloud,
                            const typename search::Search<VPoint>::Ptr & tree,
                            double tolerance,
                            std::vector<PointIndices> & clusters,
                            unsigned int min_pts_per_cluster,
                            unsigned int max_pts_per_cluster){
    if (tree->getInputCloud ()->points.size () != cloud.points.size ()){
        PCL_ERROR ("[pcl::extracteuclidean_clusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
        return;
    }
    int nn_start_idx = tree->getSortedResults () ? 1 : 0;
    std::vector<bool> processed (cloud.points.size (), false);
    std::vector<int> nn_indices;
    std::vector<float> nn_distances;

    for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)  {
        if (processed[i])
            continue;
        std::vector<int> seed_queue;
        int sq_idx = 0;
        seed_queue.push_back(i);
        processed[i] = true;
        while (sq_idx < static_cast<int> (seed_queue.size())) {

            if (!tree->nearestKSearch(seed_queue[sq_idx], sourch_k_num, nn_indices, nn_distances))  {
                sq_idx++;
                continue;
            }
            double dis = get_points2dis(cloud.points[seed_queue[sq_idx]]);
            for (size_t j = nn_start_idx; j < nn_indices.size (); ++j){
                if (nn_indices[j] == -1 || processed[nn_indices[j]])
                    continue;
                if(nn_distances[j]>max(dis_th * tolerance * dis ,tolerance))
                    continue;
                seed_queue.push_back (nn_indices[j]);
                processed[nn_indices[j]] = true;
            }
            sq_idx++;
        }

        if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size ()  max_pts_per_cluster){
            pcl::PointIndices r;
            r.indices.resize (seed_queue.size ());
            for (size_t j = 0; j < seed_queue.size (); ++j)
                r.indices[j] = seed_queue[j];

            std::sort (r.indices.begin (), r.indices.end ());
            r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

            r.header = cloud.header;
            clusters.push_back (r);
        }
    }
}

进一步分析连通域聚类算法，通过八叉树建立点云数据，采用距离聚类，并最后恢复点云标签，从而完成整个连通域聚类算法。

void OTC::octree_connection(const pcl::PointCloud<VPoint>::Ptr input_cloud,
                            std::vector<pcl::PointCloud<VPoint>>& octree_connect_cloud){
    AlignedPointTVector voxel_center_list_arg;
    voxel_center_list_arg.clear();
    octree_connect_cloud.clear();
    pcl::octree::OctreePointCloudSearch<VPoint> octree(octree_leaf_size);
    octree.setInputCloud(input_cloud);
    octree.addPointsFromInputCloud();
    octree.getOccupiedVoxelCenters(voxel_center_list_arg);

    pcl::PointCloud<VPoint> v_cloud,euc_cloud;
    v_cloud.resize(voxel_center_list_arg.size());
    for (size_t i = 0; i < voxel_center_list_arg.size(); i++){
        v_cloud[i].x = voxel_center_list_arg[i].x;
        v_cloud[i].y = voxel_center_list_arg[i].y;
        v_cloud[i].z = voxel_center_list_arg[i].z;
    }

    float dis_th = pow(pow((octree_leaf_size * 2 ),2) + pow((octree_leaf_size * 2 ),2),0.5)+0.001;
    pcl::search::KdTree<VPoint>::Ptr tree(new  pcl::search::KdTree<VPoint>);
    std::vector<pcl::PointIndices>ece_inlier;
    tree->setInputCloud(v_cloud.makeShared());
    euclidean_clusters(v_cloud,tree,dis_th, ece_inlier,1,max_points_size);
    pcl::visualization::PCLVisualizer viewer("cloud viewer");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud viewer");
    int num = 0;
    for (int i = 0; i < ece_inlier.size();i++){

        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
        pcl::PointCloud<VPoint> voxel_cloud,cloud_copy;
        pcl::copyPointCloud(v_cloud, ece_inlier_ext, cloud_copy);
        for (int j =0 ; j<cloud_copy.points.size();j++){

            std::vector<int> pointIdxVec;
            if (octree.voxelSearch(cloud_copy.points[j], pointIdxVec)){
                for (size_t k = 0; k < pointIdxVec.size(); ++k){
                    voxel_cloud.push_back(input_cloud->points[pointIdxVec[k]]);
                }
            }
        }
        if(voxel_cloud.points.size()>min_points_size){
            uint8_t R = rand() % (256) + 0;
            uint8_t G = rand() % (256) + 0;
            uint8_t B = rand() % (256) + 0;
            pcl::visualization::PointCloudColorHandlerCustom<VPoint> ramdonColor(input_cloud, R, G, B);
            viewer.addPointCloud(voxel_cloud.makeShared(), ramdonColor, std::to_string(num));
            num++;
            octree_connect_cloud.push_back(voxel_cloud);
        }
    }
    while(!viewer.wasStopped())
    {
        viewer.spinOnce();
    };
}

Octree + LCC 测试结果

测试结合地面分割算法实现，在地面分割的基础上完成连通域聚类可以有效防止错误聚类的情况发生。地面分割算法可以参考上一篇博客 [自动驾驶-目标检测] C++ PCL 地面点云分割。

; Octree + LCC 的优缺点

优点：

1. 具有很强的鲁棒性及聚类效率，不用针对整个点云数据即可实现聚类，可以适应绝大多数点云聚类场景。
2. 算法扩展性较强。距离判定的方式也可以采用其他判定方式实现，比如对八叉树分块中的局部点云特征值(反光强度分布、曲率)等等进行判别，因此可以根据实际场景进一步设计该算法。

缺点：

1. 比较依赖人工参数设定，不同的激光雷达传感器所设定的参数不一样，尤其是在点云密度分布一样的传感器其效果差别更大。
2. 无法适应于具有遮挡、重合、内部嵌套等等点云场景，这一点是现阶段传统点云聚类方法的通病。

改进思路

1. 在进行远距离的距离判定时，可以采用距离系数来进行判定，而不采用具体的值来完成，这样可以避免同一物体在远距离聚类时被划分为多个聚类点云。比如将距离阈值D t h \ D_{th}D t h ​转成d t h \ d_{th}d t h ​，利用D k / D p < d t h \ D_{k} / D_{p}< d_{th}D k ​/D p ​<d t h ​来进行判定。其中D p \ D_{p}D p ​表示起始点到坐标系远点的距离。
2. 在实践过程中，可以配合Kdtree的数据结构实现距离搜索，提高搜索效率也是可以的。

参考文献

[1] opencv connectedComponents函数
[2] Vo A V , Truong-Hong L , Laefer D F , et al. Octree-based region growing for point cloud segmentation[J]. ISPRS Journal of Photogrammetry and Remote Sensing, 2015, 104:88-100.
[3] Leon_Chan0 OpenCV学习(二十一) ：计算图像连通分量
…

Original: https://blog.csdn.net/Lion996/article/details/125221551
Author: simba丶小小程序猿
Title: [自动驾驶-目标检测] C++ PCL 连通域点云聚类