PCL 连通域点云聚类

其实就是 cloudcompare 的这个聚类功能

算法优点：

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

算法缺点：

比较依赖人工参数设定，不同的激光雷达传感器所设定的参数不一样，尤其是在点云密度分布一样的传感器其效果差别更大。

算法实现：

LabelConnectedComp.h

#pragma once

#define PCL_NO_PRECOMPILE

#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h> 


using namespace std;

class LabelConnected
{

private:

    float octree_leaf_size = 0.16f;      // 八叉树深度参数
    int max_points_size = 20000;        // 聚类最大点云数
    int min_points_size = 2;            // 聚类最小点云数
    int sourch_k_num = 10;              // 聚类K近邻搜索数
    double dis_th = 0.1;                // 聚类放大比例系数

public:

    // 设定八叉树最小深度值
    void setOctreeLeafsize(const float size)
    {
        octree_leaf_size = size;
    };

    // 设定聚类最小值
    void setMinPointSize(const int min_size)
    {
        min_points_size = min_size;
    };

    // 设定聚类比例系数
    void setDistanceTh(const double distance_th)
    {
        dis_th = distance_th;
    };


    // 欧几里得聚类算法(采用KNN搜索)
    void EuclideanCluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
        double tolerance, std::vector<pcl::PointIndices>& clusters,
        unsigned int min_pts_per_cluster,
        unsigned int max_pts_per_cluster);

    // 八叉树连通域聚类算法

    void octreeConnection(const pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
        std::vector<pcl::PointCloud<pcl::PointXYZ>>& octree_connect_cloud);
    // 聚类结果分类渲染
    void clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud);
    LabelConnected() {};

    ~LabelConnected() {};

};

LabelConnectedComp.cpp

#include "LabelConnectedComp.h"
#include <pcl/octree/octree_search.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/visualization/cloud_viewer.h>




void LabelConnected::EuclideanCluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double tolerance,
    std::vector<pcl::PointIndices>& clusters,
    unsigned int min_pts_per_cluster,
    unsigned int max_pts_per_cluster)
{
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(tolerance);// 使用分辨率初始化八叉树

    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    int nn_start_idx = 0;
    std::vector<bool> processed(cloud->size(), false);
    std::vector<int> nn_indices;
    std::vector<float> nn_distances;

    //遍历点云中的每一个点
    for (int i = 0; i < cloud->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 < seed_queue.size())  // 遍历每一个种子点
        {
            // 采用KNN搜索方式进行相邻点判定
            if (!octree.nearestKSearch(seed_queue[sq_idx], sourch_k_num, nn_indices, nn_distances))
            {
                ++sq_idx;
                continue;                // 没找到近邻点就继续
            }
            double dis = cloud->points[seed_queue[sq_idx]].getVector3fMap().norm();
            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 LabelConnected::octreeConnection(const pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
    std::vector<pcl::PointCloud<pcl::PointXYZ>>& octree_connect_cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(octree_leaf_size);
    octree.setInputCloud(input_cloud);
    octree.addPointsFromInputCloud();
    octree.getOccupiedVoxelCenters(voxelCentroids);
    // 保存八叉树体素中心为点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr v_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    v_cloud->resize(voxelCentroids.size());
    transform(voxelCentroids.begin(), voxelCentroids.end(), v_cloud->begin(), [&](const auto& p)->pcl::PointXYZ
        {
            pcl::PointXYZ point;
            point.x = p.x;
            point.y = p.y;
            point.z = p.z;
            return point;
        });






    // 欧式聚类
    float dis_th = pow(pow((octree_leaf_size * 2), 2) + pow((octree_leaf_size * 2), 2), 0.5) + 0.001;  // 计算聚类深度阈值
    std::vector<pcl::PointIndices>ece_inlier;
    EuclideanCluster(v_cloud, dis_th, ece_inlier, 1, max_points_size);       // 聚类

    for (int i = 0; i < ece_inlier.size(); i++)
    {
        //聚类完成后，需要重新找到八叉树内部所有点
        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;    // 输入所聚类到的体素中心点
        pcl::PointCloud<pcl::PointXYZ> 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)
        {
            octree_connect_cloud.push_back(voxel_cloud);
        }
    }
}
// 聚类结果分类渲染
void LabelConnected::clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud)
{
    double R = rand() % (256) + 0;
    double G = rand() % (256) + 0;
    double B = rand() % (256) + 0;

    for_each(ccloud->begin(), ccloud->end(),
        [R, G, B](pcl::PointXYZRGB& point)
        { point.r = R, point.g = G, point.b = B; });

};

main.cpp

#include "LabelConnectedComp.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("../../data/line.pcd", *cloud) == -1)
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n");
		return -1;
	}
	LabelConnected lc;
	lc.setOctreeLeafsize(0.16);
	lc.setMinPointSize(10);
	std::vector<pcl::PointCloud<pcl::PointXYZ>>label;
	lc.octreeConnection(cloud, label);

	// -----------------------聚类结果分类保存---------------------------
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusterResult(new pcl::PointCloud<pcl::PointXYZRGB>);
	int begin = 1;
	std::vector<int> idx;
	for (int i = 0; i < label.size(); ++i)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusterTemp(new pcl::PointCloud<pcl::PointXYZRGB>);
		clusterTemp->resize(label[i].size());
		for (int j = 0; j < clusterTemp->size(); ++j)
		{
			clusterTemp->points[j].x = label[i][j].x;
			clusterTemp->points[j].y = label[i][j].y;
			clusterTemp->points[j].z = label[i][j].z;
		}
		lc.clusterColor(clusterTemp);
		*clusterResult += *clusterTemp;
		// 聚类结果分类保存
		//pcl::io::savePCDFileBinary("lc_cluster_" + std::to_string(begin) + ".pcd", *clusterTemp);
		begin++;
	}

	pcl::io::savePCDFileBinary("../../data/LCclusterResult.pcd", *clusterResult);

	pcl::visualization::PCLVisualizer viewer("cloud viewer");
	viewer.setBackgroundColor(0, 0, 0);
	viewer.addPointCloud(clusterResult, "viewer");
	while (!viewer.wasStopped())//要想让自己所创窗口一直显示
	{
		viewer.spinOnce();
	}

	return 0;
}