PCL采样一致性算法

2019-12-02 14:05:13 阅读：298 来源： 互联网

标签：采样局内模型 points best PCL 一致性 model cloud

ANSAC算法的输入时一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可行的参数。

RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

有一个模型适应于假定的局内点，即所有的未知参数都能从假设的局内点计算得出；
用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为他也是局内点；
如果有足够多的点呗归类为假设的局内点，那么估计的模型就足够合理；
然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过；
最后，通过估计局内点与模型的错误率来评估模型；
这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被抛弃，要么因为比现有的模型更好而被选用。

关于模型好坏算法实现上有两种方式：

规定一个点数，达到这个点数后，算这些点与模型间的误差，找误差最小的模型。对应下面算法一
规定一个误差，找匹配模型并小于这个误差的所有点，匹配的点最多的模型，就是最好模型。对应下面算法二

伪代码一：

 1 输入：
 2 data ---- 一组观测数据
 3 model ---- 适应于数据的模型
 4 n ---- 适用于模型的最少数据个数
 5 k ---- 算法的迭代次数
 6 t ---- 用于决定数据是否适应于模型的阈值
 7 d ---- 判定模型是否适用于数据集的数据数目
 8  
 9 输出：
10 best_model —— 跟数据最匹配的模型参数（如果没有找到，返回null）
11 best_consensus_set —— 估计出模型的数据点
12 best_error —— 跟数据相关的估计出的模型的错误
13  
14 iterations = 0
15 best_model = null
16 best_consensus_set = null
17 best_error = 无穷大
18 while( iterations < k )
19     maybe_inliers =  从数据集中随机选择n个点
20     maybe_model = 适合于maybe_inliers的模型参数
21     consensus_set = maybe_inliers
22  
23     for (每个数据集中不属于maybe_inliers的点)
24         if （如果点适合于maybe_model，并且错误小于t）
25            将该点添加到consensus_set
26  
27     if (consensus_set中的点数大于d)
28         已经找到了好的模型， 现在测试该模型到底有多好
29        better_model = 适用于consensus_set中所有点的模型参数
30        this_error =  better_model 究竟如何适合这些点的度量
31     
32     if （this_error < best_error）
33         发现比以前好的模型，保存该模型直到更好的模型出现
34         best_model = better_model
35         best_consensus_set = consensus_set
36         best_error = this_error
37  
38     iterations ++
39 函数返回best_model, best_consensus_set, best_error

RANSAC算法的可能变化包括以下几种：

如果发现一种足够好的模型（该模型有足够下的错误率），则跳出主循环，这样节约不必要的计算；设置一个错误率的阈值，小于这个值就跳出循环；
可以直接从maybe_model计算this_error，而不从consensus_set重新估计模型，这样可能会节约时间，但是可能会对噪音敏感。
伪代码二：

 1 输入：
 2 data ---- 一组观测数据
 3 numForEstimate ----- 初始模型需要的点数
 4 delta ------ 判定点符合模型的误差
 5 probability ----- 表示迭代过程中从数据集内随机选取出的点均为局内点的概率
 6  
 7 输出：
 8 best_model —— 跟数据最匹配的模型参数（如果没有找到，返回null）
 9 best_consensus_set —— 估计出模型的数据点
10  
11 k = 1000    //设置初始值
12  
13 iterations = 0
14 best_model = null
15 best_consensus_set = null
16  
17 while( iterations < k )
18     maybe_inliers =  从数据集中随机选择numForEstimate个点
19     maybe_model = 适合于maybe_inliers的模型参数，比如直线，取两个点，得直线方程
20  
21     for (每个数据集中不属于maybe_inliers的点)
22         if （如果点适合于maybe_model，并且错误小于delta）
23             将该点添加到maybe_inliers
24  
25     if(maybe_inliers的点数 > best_consensus_set 的点数）    //找到更好的模型
26         best_model = maybe_model
27         best_consensus_set  = maybe_inliers
28         根据公式k=log(1-p)/log(1-pow(w,n))重新计算k
29     iterations ++
30 函数返回best_model, best_consensus_set,

#include <iostream>
#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>
#include <boost/thread/thread.hpp>

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> 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);
}
/******************************************************************************************************************
 对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算
  而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的组委局外点。
 *****************************************************************************************************************/
int
main(int argc, char** argv)
{
    // 初始化点云对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点

    // 填充点云数据
    cloud->width = 5000;                 //填充点云数目
    cloud->height = 1;                     //无序点云
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    for (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)
        {
            //根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点
            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
        { //用x+y+z=1设置一部分点云数据，此时地拿云组成的菱形平面作为内点
            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;  //存储局内点集合的点的索引的向量

    //创建随机采样一致性对象
    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);                 //存储估计所得的局内点
    }
    else if (pcl::console::find_argument(argc, argv, "-sf") >= 0)
    {
        //根据命令行参数  来随机估算对应的圆球模型，存储估计的内点
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
        ransac.setDistanceThreshold(.01); //与球面距离小于0.01 的点称为局内点考虑
        ransac.computeModel();  //执行随机参数估计
        ransac.getInliers(inliers); //存储估计所得的局内点
    }

    //复制估算模型的所有的局内点到final中
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
    // 创建可视化对象并加入原始点云或者所有的局内点
    boost::shared_ptr<pcl::visualization::PCLVisualizer> 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);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    return 0;
}

标签：采样,局内,模型,points,best,PCL,一致性,model,cloud
来源： https://www.cnblogs.com/hsy1941/p/11970211.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

PCL采样一致性算法