PCL八叉树聚类

主要流程

• ​读取点云数据：从PCD文件中加载原始点云
  
• ​构建八叉树：对点云进行八叉树空间划分
  
• ​获取体素中心：提取八叉树中所有被占据的体素中心点
  
• ​欧式聚类：对体素中心点进行欧式聚类
  
• ​扩展聚类结果：将聚类结果从体素中心扩展到原始点云
  
• ​可视化与保存：对聚类结果着色并可视化/保存
  

完整代码

1. #include <pcl/io/pcd_io.h>
2. #include <pcl/point_types.h>
3. #include <pcl/octree/octree_search.h>
4. #include <pcl/octree/octree_pointcloud.h>
5. #include <pcl/segmentation/extract_clusters.h> // 欧式聚类分割
6. #include <pcl/visualization/pcl_visualizer.h>
8. // 聚类结果分类渲染
9. void clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud)
10. {
11.         double R = rand() % (256) + 0;
12.         double G = rand() % (256) + 0;
13.         double B = rand() % (256) + 0;
15.         for_each(ccloud->begin(), ccloud->end(),
16.                 [R, G, B](pcl::PointXYZRGB& point)
17.                 { point.r = R, point.g = G, point.b = B; });
19. };
22. int main(int argc, char* argv[])
23. {
24.         // --------------------------------读取点云------------------------------------
25.         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
26.         if (pcl::io::loadPCDFile<pcl::PointXYZ>("../../../data/000000.pcd", *cloud) == -1)
27.         {
28.                 PCL_ERROR("Couldn't read file test_pcd.pcd \n");
29.                 return -1;
30.         }
31.         // 参数设置
32.         float leaf = 0.3f;     // 八叉树深度参数
33.         int minSize = 50;
34.         // --------------------------获取八叉树体素中心-------------------------------
35.         pcl::PointCloud<pcl::PointXYZ>::VectorType voxelCentroids;
36.         pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(leaf);
37.         octree.setInputCloud(cloud);
38.         octree.addPointsFromInputCloud();
39.         octree.getOccupiedVoxelCenters(voxelCentroids);
40.         // 保存八叉树体素中心为点云
41.         pcl::PointCloud<pcl::PointXYZ>::Ptr v_cloud(new pcl::PointCloud<pcl::PointXYZ>);
42.         v_cloud->resize(voxelCentroids.size());
43.         transform(voxelCentroids.begin(), voxelCentroids.end(), v_cloud->begin(), [&](const auto& p)->pcl::PointXYZ
44.                 {
45.                         pcl::PointXYZ point;
46.                         point.x = p.x;
47.                         point.y = p.y;
48.                         point.z = p.z;
49.                         return point;
50.                 });
52.         float dis_th = std::sqrt(3.0f * leaf * leaf); // 计算聚类深度阈值
53.         // ------------------------------欧式聚类------------------------------------
54.         pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
55.         tree->setInputCloud(v_cloud);
56.         std::vector<pcl::PointIndices> cluster_indices;   // 聚类索引
57.         pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;// 欧式聚类对象
58.         ec.setClusterTolerance(dis_th);                   // 设置近邻搜索的搜索半径（也即两个不同聚类团点之间的最小欧氏距离）
59.         ec.setMinClusterSize(minSize);                    // 设置一个聚类需要的最少的点数
60.         ec.setMaxClusterSize(v_cloud->size());            // 设置一个聚类需要的最大点数
61.         ec.setSearchMethod(tree);                         // 设置点云的搜索机制
62.         ec.setInputCloud(v_cloud);                        // 设置输入点云
63.         ec.extract(cluster_indices);                      // 从点云中提取聚类，并将点云索引保存在cluster_indices中
65.         std::vector<pcl::PointCloud<pcl::PointXYZ>>label;
66.         // ---------------------------最终聚类结果----------------------------------
67.         for (int i = 0; i < cluster_indices.size(); i++)
68.         {
69.                 // 聚类完成后，重新找到八叉树内部所有点
70.                 pcl::PointCloud<pcl::PointXYZ> voxel_cloud, cloud_copy;
71.                 pcl::copyPointCloud(*v_cloud, cluster_indices[i].indices, cloud_copy);   // 按照索引提取点云数据
72.                 for (int j = 0; j < cloud_copy.points.size(); ++j)
73.                 {
74.                         std::vector<int> pointIdxVec;                           // 保存体素近邻搜索的结果向量
75.                         if (octree.voxelSearch(cloud_copy.points[j], pointIdxVec))
76.                         {
77.                                 for (size_t k = 0; k < pointIdxVec.size(); ++k)
78.                                 {
79.                                         voxel_cloud.push_back(cloud->points[pointIdxVec[k]]);
80.                                 }
81.                         }
82.                 }
83.                 if (voxel_cloud.points.size() > minSize)
84.                 {
85.                         label.push_back(voxel_cloud);
86.                 }
87.         }
89.         // -----------------------聚类结果分类保存---------------------------
90.         pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusterResult(new pcl::PointCloud<pcl::PointXYZRGB>);
91.         int begin = 1;
92.         std::vector<int> idx;
93.         for (int i = 0; i < label.size(); ++i)
94.         {
95.                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusterTemp(new pcl::PointCloud<pcl::PointXYZRGB>);
96.                 clusterTemp->resize(label[i].size());
97.                 for (int j = 0; j < clusterTemp->size(); ++j)
98.                 {
99.                         clusterTemp->points[j].x = label[i][j].x;
100.                         clusterTemp->points[j].y = label[i][j].y;
101.                         clusterTemp->points[j].z = label[i][j].z;
102.                 }
103.                 clusterColor(clusterTemp);
104.                 *clusterResult += *clusterTemp;
105.                 // 聚类结果分类保存
106.                 //pcl::io::savePCDFileBinary("lc_cluster_" + std::to_string(begin) + ".pcd", *clusterTemp);
107.                 begin++;
108.         }
110.         pcl::io::savePCDFileBinary("LCclusterResult.pcd", *clusterResult);
112.         pcl::visualization::PCLVisualizer viewer("cloud viewer");
113.         viewer.setBackgroundColor(0, 0, 0);
114.         viewer.addPointCloud(clusterResult, "viewer");
115.         while (!viewer.wasStopped())//要想让自己所创窗口一直显示
116.         {
117.                 viewer.spinOnce();
118.         }
120.         return 0;
121. }

复制代码

部分代码剖析

//聚类结果分类渲染

1. void clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud)
2. {
3.         double R = rand() % (256) + 0;
4.         double G = rand() % (256) + 0;
5.         double B = rand() % (256) + 0;
7.         for_each(ccloud->begin(), ccloud->end(),
8.                 [R, G, B](pcl::PointXYZRGB& point)
9.                 { point.r = R, point.g = G, point.b = B; });
11. };

复制代码

关键元素剖析

std::for_each算法

• ​功能：遍历从ccloud->begin()到ccloud->end()的所有点
  
• ​作用：对点云中的每个点执行指定的lambda函数
  
• ​特点：
  
  
  
  • 标准模板库(STL)提供的遍历算法
    
  • 比传统for循环更简洁安全
    
  • 自动处置惩罚迭代器范围
    
  
  

Lambda表达式

1. [R, G, B](pcl::PointXYZRGB& point) {
2.     point.r = R,
3.     point.g = G,
4.     point.b = B;
5. }

复制代码

等价

1. void clusterColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& ccloud) {
2.     uint8_t R = rand() % 256;
3.     uint8_t G = rand() % 256;
4.     uint8_t B = rand() % 256;
5.    
6.     for(size_t i = 0; i < ccloud->size(); ++i) {
7.         (*ccloud)[i].r = R;
8.         (*ccloud)[i].g = G;
9.         (*ccloud)[i].b = B;
10.     }
11. }

复制代码

结果

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。