版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/HERO_CJN/article/details/84397862
算法核心:该算法是基于点法线之间角度的比较,企图将满足平滑约束的相邻点合并在一起,以一簇点集的形式输出。每簇点集被认为是属于相同平面。
区域增长算法已经是比较老的平面分割算法
void people2D_engine::pointcloud_cluster( const laserscan_data& point_cloud, std::vector<pcl::PointIndices>& clusters){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for(int i = 0; i < point_cloud.data.pts.size(); ++i){
pcl::PointXYZ point;
point.x = point_cloud.data.pts[i].x;
point.y = point_cloud.data.pts[i].y;
point.z = 0;
cloud->push_back(point);
}
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(50);
normal_estimator.compute(*normals);
pcl::IndicesPtr indices(new std::vector <int>);
// 给定点云某个字段限定的对点云进行简单的过滤
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 1.0);
pass.filter(*indices);
// 区域生长点云分割类
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
// 设定每个类中最少点数
reg.setMinClusterSize(3);
reg.setMaxClusterSize(1000000);
reg.setSearchMethod(tree);
// 设置领域的数量
reg.setNumberOfNeighbours(30);
reg.setInputCloud(cloud);
reg.setInputNormals(normals);
// 设置平滑阈值大小
reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
// 设置曲率阈值
reg.setCurvatureThreshold(1.0);
reg.extract(clusters);
reg.getColoredCloud();
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
//pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
std::cout << "Number of clusters is equal to " << cloud_clusters.size() << std::endl;
//std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
}