PCL八叉树聚类
#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;
}