PCL点云处理(007)-Ransac

news2024/11/25 7:07:16

随机抽样一致性算法RANSAC(Random sample consensus)是一种迭代的方法来从一系列包含有离异值的数据中计算数学模型参数的方法。
RANSAC算法本质上由两步组成，不断进行循环：
从输入数据中随机选出能组成数学模型的最小数目的元素，使用这些元素计算出相应模型的参数。选出的这些元素数目是能决定模型参数的最少的。
检查所有数据中有哪些元素能符合第一步得到的模型。超过错误阈值的元素认为是离群值（outlier），小于错误阈值的元素认为是内部点（inlier）。
这个过程重复多次，选出包含点最多的模型即得到最后的结果。
RANSAC具体到空间点云中拟合平面：
1、从点云中随机选取三个点。
2、由这三个点组成一个平面。
3、计算所有其他点到该平面的距离，如果小于阈值T，就认为是处在同一个平面的点。
3、如果处在同一个平面的点超过n个，就保存下这个平面，并将处在这个平面上的点都标记为已匹配。
4、终止的条件是迭代N次后找到的平面小于n个点，或者找不到三个未标记的点。
下面是一个使用PCL中的Ransac进行平面拟合的示例代码：

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h> // 拟合平面
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

int main()
{
    //-----------------------------读取点云----------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile("../../data/example4.pcd", *cloud) < 0)
    {
        PCL_ERROR("点云读取失败！\n");
        return -1;
    }
    //--------------------------RANSAC拟合平面--------------------------
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);
    ransac.setDistanceThreshold(0.2);	//设置距离阈值，与平面距离小于0.1的点作为内点
    ransac.computeModel();				//执行模型估计
    //-------------------------根据索引提取内点--------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
    vector<int> inliers;				//存储内点索引的容器
    ransac.getInliers(inliers);			//提取内点索引
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_plane);
    //----------------------------输出模型参数---------------------------
    Eigen::VectorXf coefficient;
    ransac.getModelCoefficients(coefficient);
    cout << "平面方程为：\n" << coefficient[0] << "x + " << coefficient[1] << "y + " << coefficient[2] << "z + "
         << coefficient[3] << " = 0" << endl;
    //-----------------------------结果可视化----------------------------
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");

    viewer->addPointCloud<pcl::PointXYZ>(cloud_plane, "plane");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }

    return 0;
}