基于XYZRGB点云实现欧式聚类分割算法C++代码

一.算法效果

点云聚类算法，顾名思义，就是找出点云中相同类型的点，这些点称为一个个簇，也就是说点云聚类是找出点云中相同类型的点簇，类型包括距离，颜色，法相等。欧式聚类算法基于点之间的欧式距离来对点云进行聚类，算法效果如下：

图1点云欧式聚类分割效果图

二.算法原理

分割思路

欧式聚类以一个点p开始，设置一个距离，遍历点云中点q，若点p,q间欧式距离小于设定距离的，则点p,q是同一个类型，将其加入p点所在点云簇，重复上述步骤，直到找不到满足距离条件的点为止，这样就得到了一个簇。依次类推，迭代点云中其它的点，最后点云就会分割成一个个的簇。对点云簇进行过滤，去掉点数过多或者过少的簇，即可获得需要的点云簇。

算法伪代码

1. 设置搜索半径d,最小点数阈值min_n,最大点数阈值max_n;
2. 为点云P创建KD-Tree搜索树;
4. 创建空的聚类列表C和点云的检查队列Q;
5. 对于P中的每一个点Pi，执行如下操作：
6.    - 将Pi添加到当前队列Q, 并将Pi标记为已处理;
7.    - while处理 Q 中的每一个Pi：
8.          - 对Pi进行近邻搜索，查找满足半径 < d 的点集合;
9.          - 检查上一步中每一个邻居点，如果标记是未被处理的点，则加入到队列Q中;
10.    - 直到Q中的所有点都被标记为已处理，将Q加入到聚类列表C中，将Q重新置为空
11. 当所有的Pi都被处理过之后结束，聚类结果为列表C;
12. 对点云簇C进行过滤，去掉点数过多或者过少的簇，即可获得满足条件的点云簇。

三.C++代码实现

1.环境配置

VS2019 + PCL1.12.1

2.C++代码

1.头文件 EuclideanClusters.h

#include 
#include 
#include 
#include  

class EuclideanClusters
{
public:
    typedef std::shared_ptr<EuclideanClusters> Ptr;
    typedef std::shared_ptr<const EuclideanClusters>ConstPtr;
    using KdTree = pcl::search::Search<pcl::PointXYZRGB>;
    using KdTreePtr = typename KdTree::Ptr;

    EuclideanClusters();
    ~EuclideanClusters() {}

    //输入点云
    void
    setInputCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in)
    {
        cloud_in_ = cloud_in;
    }
   
    //设置近邻点搜索方法
    inline void
    setSearchMethod(const KdTreePtr& tree)
    {
        tree_ = tree;
    }

    //设置点云簇间欧式距离容差
    inline void
    setClusterTolerance(double tolerance)
    {
        cluster_tolerance_ = tolerance;
    }

    //设置最小聚类点数
    inline void
    setMinClusterSize(pcl::uindex_t min_cluster_size)
    {
        min_pts_per_cluster_ = min_cluster_size;
    }

    //设置最大聚类点数
    inline void
    setMaxClusterSize(pcl::uindex_t max_cluster_size)
    {
        max_pts_per_cluster_ = max_cluster_size;
    }

    //获取错误消息
    std::string getErrorMessage()
    {
        return error_msg_;
    }

    //调用接口，输出点云簇的id向量
    bool
    extract(std::vector<pcl::PointIndices>& clusters);

private:
    //input pameters
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in_;
    KdTreePtr tree_;
    float cluster_tolerance_;
    pcl::uindex_t min_pts_per_cluster_;
    pcl::uindex_t max_pts_per_cluster_;

    //error message
    std::string error_msg_;

};

2.源文件 EuclideanClusters.cpp

#include "segment/EuclideanClusters.h"

inline bool
comparePointClusters(const pcl::PointIndices& a, const pcl::PointIndices& b)
{
	return (a.indices.size() < b.indices.size());
}

EuclideanClusters::EuclideanClusters()
{
	cloud_in_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
}

bool 
EuclideanClusters::extract(std::vector<pcl::PointIndices>& clusters)
{
	// Check if the tree is sorted -- if it is we don't need to check the first element
	int nn_start_idx = tree_->getSortedResults() ? 1 : 0;

	pcl::IndicesPtr indices(new pcl::Indices);
	indices->resize(cloud_in_->size());

	// Create a bool vector of processed point indices, and initialize it to false
	std::vector<bool> processed(cloud_in_->size(), false);

	pcl::Indices nn_indices;
	std::vector<float> nn_distances;
	// Process all points in the indices vector
	for (int index = 0; index < cloud_in_->size(); index++)
	{
		if (processed[index])
			continue;

		pcl::Indices seed_queue;
		int sq_idx = 0;
		seed_queue.push_back(index);

		processed[index] = true;

		while (sq_idx < static_cast<int> (seed_queue.size()))
		{
			// Search for sq_idx
			int ret = tree_->radiusSearch(cloud_in_->at(seed_queue[sq_idx]), cluster_tolerance_, nn_indices, nn_distances);
			if (ret == -1)
			{
				error_msg_ = "Received error code -1 from radiusSearch\n";
				return false;
			}
			if (!ret)
			{
				sq_idx++;
				continue;
			}

			for (std::size_t j = nn_start_idx; j < nn_indices.size(); ++j)             // can't assume sorted (default isn't!)
			{
				if (nn_indices[j] == pcl::UNAVAILABLE || processed[nn_indices[j]])        // Has this point been processed before ?
					continue;

				// Perform a simple Euclidean clustering
				seed_queue.push_back(nn_indices[j]);
				processed[nn_indices[j]] = true;
			}

			sq_idx++;
		}

		// If this queue is satisfactory, add to the clusters
		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 (std::size_t j = 0; j < seed_queue.size(); ++j)
				// This is the only place where indices come into play
				r.indices[j] = seed_queue[j];

			// These two lines should not be needed: (can anyone confirm?) -FF
			//r.indices.assign(seed_queue.begin(), seed_queue.end());
			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_in_->header;
			clusters.push_back(r);   // We could avoid a copy by working directly in the vector
		}
	}

	// Sort the clusters based on their size (largest one first)
	std::sort(clusters.rbegin(), clusters.rend(), comparePointClusters);

	if (clusters.empty())
	{
		error_msg_ = "Euclidean clusters segment error.\n";
		return false;
	}

	return true;
}

3.主函数 main.cpp

#include 
//io
#include 
#include 
#include 
#include 

#include "point_cloud/include/io/cloud_io.h"
#include "point_cloud/include/segment/EuclideanClusters.h"

bool euclideanClusterExtraction(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in,
	std::vector <pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_out_vec);

int main(int argc, char *argv[])
{
	//load cloud
	std::string file_name = "..\\Data\\clouds\\test.csv";
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZRGB>());

	if (!io::loadCSV2PCDXYZRGB(file_name, cloud_in))
	{
		std::cout << "Loaded cloud error!" << std::endl;
		return -1;
	}
	std::cout << "Loaded cloud " << cloud_in->size() << " data points. " << std::endl;

	std::vector <pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloud_hull_vec;
	if (!euclideanClusterExtraction(cloud_in, cloud_hull_vec))return false;

	//viewer
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));

	std::string str = "cloud_in";
	//pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud_in); //使用cloud的rgb 或者 rgba
	//viewer->addPointCloud(cloud_in, rgb, str);
	//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, str); //变量名，赋值
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(cloud_in, 255.0, 255.0, 255.0); 
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud_in, single_color, str);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, str);

	for (size_t i = 0; i < cloud_hull_vec.size(); i++)
	{
		std::string str = "cloud_" + std::to_string(i);

		//generate random color
		int randomR = rand() % 255;
		int randomG = rand() % 255;
		int randomB = rand() % 255;
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> single_color(cloud_hull_vec[i], randomR, randomG, randomB); 

		viewer->addPointCloud<pcl::PointXYZRGB>(cloud_hull_vec[i], single_color, str);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, str);
	}

	while (!viewer->wasStopped())
	{
		viewer->spinOnce();
	}

	return 0;
}

bool euclideanClusterExtraction(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in,
	std::vector <pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_out_vec)
{
	// 创建kd树
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree->setInputCloud(cloud_in);

	// 设置分割参数
	std::vector<pcl::PointIndices> cluster_indices;
	EuclideanClusters ec;
	ec.setClusterTolerance(0.02);
	ec.setMinClusterSize(1);
	ec.setMaxClusterSize(INT_MAX);
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloud_in);
	ec.extract(cluster_indices);

	if (cluster_indices.empty())
	{
		std::cout << "Solder cloud segment error.\n" << std::endl;
		return false;
	}

	// 执行欧式聚类分割，并保存分割结果
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
		std::vector<float> distances_single;
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
		{
			cloud_cluster->points.push_back(cloud_in->points[*pit]);
		}
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		cloud_out_vec.push_back(cloud_cluster);
	}
	return true;
}