PCL点云库调库学习系列——随机采样一致性（附完整代码）

随机采样一致性

实现功能

从输入点云中，找到符合某种模型的点云集

具体为：随机产生一系列点云，创建两个随机采样一致性模型为球和平面，根据参数的不同选择不同的模型，执行参数估计，可以得到模型中的局内点，最后将结果可视化显示出来

关键函数

//1.随机采样执行流程
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
        model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));  //1平面模型
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);  //2随机采样一致性对象
ransac.setDistanceThreshold(.01);       //3设置距离阈值，小于0.01的点作为局内点考虑
ransac.computeModel();                  //4执行随机参数估计
ransac.getInliers(inliers);             //5获取所得内点，结果保存在inliers中

//复制局内点点云
pcl::copyPointCloud(*cloud, inliers, *final);	//将cloud点云中索引为inliers中所有点复制到final中
//另外一个常用的重载版本的函数
//将输入点云复制到输出点云中
void pcl::copyPointCloud(const pcl::PointCloud<PointInT>& cloud_in,  //输入点云
  pcl::PointCloud<PointOutT>& cloud_out  //输出点云
 ) 
 

完整代码

#include <iostream>
#include <thread>

#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

pcl::visualization::PCLVisualizer::Ptr
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    
    
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    //点云显示
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    //viewer->addCoordinateSystem (1.0, "global");
    viewer->initCameraParameters();
    return (viewer);
}

int
main(int argc, char** argv)
{
    
    
    // initialize PointClouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

    // populate our PointCloud with points
    cloud->width = 500;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    //生成点云数据
    for (std::size_t i = 0; i < cloud->points.size(); ++i)
    {
    
    
        if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
        {
    
    
            cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
            if (i % 5 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
            else if (i % 2 == 0)
                cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
                    - (cloud->points[i].y * cloud->points[i].y));
            else
                cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
                    - (cloud->points[i].y * cloud->points[i].y));
        }
        else
        {
    
    
            cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
            if (i % 2 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
            else
                cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
        }
    }

    std::vector<int> inliers;

    // created RandomSampleConsensus object and compute the appropriated model
    //创建随机采样一致性对象，并计算合适的模型
    pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
        model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud)); //球模型
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
        model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));  //平面模型
    if (pcl::console::find_argument(argc, argv, "-f") >= 0)
    {
    
    
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);  //随机采样一致性对象
        ransac.setDistanceThreshold(.01);       //设置距离阈值，小于0.01的点作为局内点考虑
        ransac.computeModel();                  //执行随机参数估计
        ransac.getInliers(inliers);             //获取所得内点，结果保存在inliers中
    }
    else if (pcl::console::find_argument(argc, argv, "-sf") >= 0)
    {
    
    
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
        ransac.setDistanceThreshold(.01);
        ransac.computeModel();
        ransac.getInliers(inliers);
    }

    // copies all inliers of the model computed to another PointCloud
    //将模型中所有局内点复制到final中
    pcl::copyPointCloud(*cloud, inliers, *final);

    // creates the visualization object and adds either our original cloud or all of the inliers
    // depending on the command line arguments specified.
    //创建可视化对象
    pcl::visualization::PCLVisualizer::Ptr viewer;
    if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
        viewer = simpleVis(final);
    else
        viewer = simpleVis(cloud);
    while (!viewer->wasStopped())
    {
    
    
        viewer->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
    return 0;
}

运行结果

无参数输入时

输入参数-f时