Analysis of PCL source code – European clustering
References :
1. pcl Euclidean Cluster Extraction tutorial
2. Euclidean cluster analysis
3. pcl-api source code
4. Point cloud European clustering
5. Complete project address of this article
visualize the results
1. Theory
Clustering methods need to divide the unorganized point cloud model P into smaller parts in order to significantly reduce the overall processing time of P. Simple data clustering methods in Euclidean space can be implemented using 固定宽度box3D meshing or general 八叉树data structures. This particular representation is fast and occupies space in the voxel representation. Then, the more commonly used method is to use 最近邻clustering techniques, which are similar in nature to 洪水填充（flood fill）algorithms.
2. Pseudocode
1. 为点云P 创建KD-Tree的输入表征
2. 创建空的聚类列表C 和 点云的检查队列Q
3. 对于P中的每一个点Pi，执行如下操作：
4. - 将Pi添加到当前队列Q(并标记为已处理)；
5. - while处理 Q 中的每一个Pi：
6. - 对Pi进行近邻搜索，查找满足半径 < d 的点集合;
7. - 检查上一步中每一个邻居点，如果标记是未被处理的点，则加入到队列Q中；
8. - 直到Q中的所有点都被标记为已处理，将Q加入到聚类列表C中，将Q重新置为空
9. 当所有的Pi都被处理过之后结束，聚类结果为列表 C
3. Source code analysis
3.1 Some explanations about the main function extractEuclideanClusters
- Function Description
According to the maximum and minimum number of clustering points and the threshold of European clustering, the point cloud is clustered in European style. - Parameter Description
- cloud: 输入点云
- tree: kd-tree
- tolerance: 欧式聚类距离阈值
- clusters: 输出参数，最终聚类结果数组
- min_pts_per_cluster： 每个聚类结果最小点数
- max_pts_per_cluster： 每个聚类结果最大点数
3.2 Implementation of key function extractEuclideanClusters
Comments have been added to the source code
#include <pcl/segmentation/extract_clusters.h> // 分割模块
#include <pcl/search/organized.h> // for OrganizedNeighbor
//
template <typename PointT> void
pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters,
unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster)
{
// 若构建的kd-tree中点数，与输入点云数据点数不同，直接错误打印，并退出
if (tree->getInputCloud ()->size () != cloud.size ())
{
PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
"dataset (%zu) than the input cloud (%zu)!\n",
static_cast<std::size_t>(tree->getInputCloud()->size()),
static_cast<std::size_t>(cloud.size()));
return;
}
// 检查kdtree是否经过排序，若结果为true，则不需要检查第一个元素；否则需要检查第一个元素，默认为0
int nn_start_idx = tree->getSortedResults () ? 1 : 0;
// 建立处理队列Q，数目为点数数目，每一个值为false
std::vector<bool> processed (cloud.size (), false);
// nn_indices代表某一个点knn近邻查找，满足距离阈值的点云索引
Indices nn_indices;
std::vector<float> nn_distances;
// 遍历每一个点
for (int i = 0; i < static_cast<int> (cloud.size ()); ++i)
{
// 若该点云被标记为true(处理过)，则跳过该点
if (processed[i])
continue;
// 当前某一个聚类的种子队列Q
Indices seed_queue;
int sq_idx = 0; // 种子队列索引
seed_queue.push_back (i); // 将当前点加入种子队列，并标记为true
processed[i] = true;
// 循环处理种子队列Q中的每一个点，进行K近邻查找以及标记工作
while (sq_idx < static_cast<int> (seed_queue.size ()))
{
// 对种子队列中索引的点进行半径距离查找，若半径内没有可查询的点，则索引+1, 跳过后续处理
if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}
// 半径内有满足条件的点，点云索引保存在nn_indices。遍历这些点，判断是否处理过，若没有处理过，则加入到种子队列Q中
for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
{
// 若该索引为0 或者标记为被处理过之后，则跳过该点
if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]]) // Has this point been processed before ?
continue;
// 进行简单的欧式聚类，即将其加入到种子队列中，并标记为true
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; // PointIndices类中indices是一个vector
r.indices.resize (seed_queue.size ());
// 完成种子队列中索引的赋值操作
for (std::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);
}
else
{
// 打印聚类数目超限
PCL_DEBUG("[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
}
}
}
4. Application
Test data table_scene_lms400.pcd Download
the application Complete project address
analysis : The overall process of the application has undergone downsampling, using the plane model to divide the desktop, filtering multiple planes; performing European clustering on the remaining point clouds, and visualizing the European clustering results.
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include<pcl/visualization/pcl_visualizer.h>
bool isPushSpace = false;
//键盘事件
void keyboard_event_occurred(const pcl::visualization::KeyboardEvent& event, void * nothing)
{
if (event.getKeySym() == "space" && event.keyDown())
{
isPushSpace = true;
}
}
int main(int argc, char** argv)
{
// 从PCD文件中读取点云数据
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
reader.read("../../../data/table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
pcl::visualization::PCLVisualizer viewer("Cluster Extraction");
// 注册键盘事件
viewer.registerKeyboardCallback(&keyboard_event_occurred, (void*)NULL);
int v1(1);
int v2(2);
viewer.createViewPort(0, 0, 0.5, 1, v1);
viewer.createViewPort(0.5, 0, 1, 1, v2);
// 使用下采样，分辨率 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
viewer.addPointCloud(cloud, "cloud1", v1);
viewer.addPointCloud(cloud_filtered, "cloud2", v2);
//渲染10秒再继续
viewer.spinOnce(10000);
// 创建平面分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
// 把点云中所有的平面全部过滤掉，重复过滤，直到点云数量小于原来的0.3倍
int i = 0, nr_points = (int)cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Write the planar inliers to disk
extract.filter(*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
//更新显示点云
viewer.updatePointCloud(cloud_filtered, "cloud1");
viewer.updatePointCloud(cloud_f, "cloud2");
//渲染3秒再继续
viewer.spinOnce(3000);
cloud_filtered = cloud_f;
}
viewer.removePointCloud("cloud2", v2);
/// 创建KdTree对象作为搜索方法
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_filtered);
/// 欧式聚类
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02); // 2cm
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
//遍历抽取结果，将其显示并保存
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
//创建临时保存点云族的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
//通过下标，逐个填充
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*
//设置点云属性
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "当前聚类 "<<j<<" 包含的点云数量: " << cloud_cluster->points.size() << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
// writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
j++;
//显示,随机设置不同颜色，以区分不同的聚类
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cluster_color(cloud_cluster, rand()*100 + j * 80, rand() * 50 + j * 90, rand() * 200 + j * 100);
viewer.addPointCloud(cloud_cluster,cluster_color, ss.str(), v2);
viewer.spinOnce(5000);
}
while (!viewer.wasStopped())
{
viewer.spinOnce();
}
return (0);
}