PCL 区域生长算法

算法核心：该算法是基于点法线之间角度的比较，企图将满足平滑约束的相邻点合并在一起，以一簇点集的形式输出。每簇点集被认为是属于相同平面。

区域增长算法已经是比较老的平面分割算法

参考：http://pointclouds.org/documentation/tutorials/region_growing_segmentation.php#region-growing-segmentation

 

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;
}