PCL八叉树聚类
主要流程
- 读取点云数据：从PCD文件中加载原始点云
- 构建八叉树：对点云进行八叉树空间划分
- 获取体素中心：提取八叉树中所有被占据的体素中心点
- 欧式聚类：对体素中心点进行欧式聚类
- 扩展聚类结果：将聚类结果从体素中心扩展到原始点云
- 可视化与保存：对聚类结果着色并可视化/保存
完整代码
cpp
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree_search.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/segmentation/extract_clusters.h> // 欧式聚类分割
#include <pcl/visualization/pcl_visualizer.h>
// 聚类结果分类渲染
void 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; });
};
int main(int argc, char* argv[])
{
// --------------------------------读取点云------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("../../../data/000000.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return -1;
}
// 参数设置
float leaf = 0.3f; // 八叉树深度参数
int minSize = 50;
// --------------------------获取八叉树体素中心-------------------------------
pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(leaf);
octree.setInputCloud(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 = std::sqrt(3.0f * leaf * leaf); // 计算聚类深度阈值
// ------------------------------欧式聚类------------------------------------
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(v_cloud);
std::vector<pcl::PointIndices> cluster_indices; // 聚类索引
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;// 欧式聚类对象
ec.setClusterTolerance(dis_th); // 设置近邻搜索的搜索半径（也即两个不同聚类团点之间的最小欧氏距离）
ec.setMinClusterSize(minSize); // 设置一个聚类需要的最少的点数
ec.setMaxClusterSize(v_cloud->size()); // 设置一个聚类需要的最大点数
ec.setSearchMethod(tree); // 设置点云的搜索机制
ec.setInputCloud(v_cloud); // 设置输入点云
ec.extract(cluster_indices); // 从点云中提取聚类，并将点云索引保存在cluster_indices中
std::vector<pcl::PointCloud<pcl::PointXYZ>>label;
// ---------------------------最终聚类结果----------------------------------
for (int i = 0; i < cluster_indices.size(); i++)
{
// 聚类完成后，重新找到八叉树内部所有点
pcl::PointCloud<pcl::PointXYZ> voxel_cloud, cloud_copy;
pcl::copyPointCloud(*v_cloud, cluster_indices[i].indices, 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(cloud->points[pointIdxVec[k]]);
}
}
}
if (voxel_cloud.points.size() > minSize)
{
label.push_back(voxel_cloud);
}
}
// -----------------------聚类结果分类保存---------------------------
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;
}
clusterColor(clusterTemp);
*clusterResult += *clusterTemp;
// 聚类结果分类保存
//pcl::io::savePCDFileBinary("lc_cluster_" + std::to_string(begin) + ".pcd", *clusterTemp);
begin++;
}
pcl::io::savePCDFileBinary("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;
}部分代码解析
//聚类结果分类渲染
cpp
void 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; });
};关键元素解析
std::for_each算法
- 功能 ：遍历从
ccloud->begin()到ccloud->end()的所有点 - 作用：对点云中的每个点执行指定的lambda函数
- 特点 ：
- 标准模板库(STL)提供的遍历算法
- 比传统for循环更简洁安全
- 自动处理迭代器范围
Lambda表达式
cpp
[R, G, B](pcl::PointXYZRGB& point) {
point.r = R,
point.g = G,
point.b = B;
}等价
cpp
void clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud) {
uint8_t R = rand() % 256;
uint8_t G = rand() % 256;
uint8_t B = rand() % 256;
for(size_t i = 0; i < ccloud->size(); ++i) {
(*ccloud)[i].r = R;
(*ccloud)[i].g = G;
(*ccloud)[i].b = B;
}
}效果
