ransac平面拟合

pcl::RandomSampleConsensus是RANSAC算法的实现。pcl::SampleConsensusModelPlane实现采样一致计算的三维平面模型分割类。
下面一段代码演示利用上述两个类拟合空间平面方程。

1. 首先生成点云数据，设定空间平面方程为x +y+z=1，其中每隔五个点生成一个点作为噪声点。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 10000;
    cloud->height = 1;
    cloud->is_dense = FALSE;
    cloud->resize(cloud->width * cloud->height);

    // 生成平面x2 + y2 + z2 = 1的点云数据
    size_t num = cloud->size();
    for (size_t i = 0 ; i < num ; ++i)
    {
        cloud->points[i].x = rand() / (RAND_MAX + 1.0);
        cloud->points[i].y = rand() / (RAND_MAX + 1.0);

        // 生成局外点
        if (i % 5 == 0)
        {
            cloud->points[i].z = rand()/(RAND_MAX + 1.0) ;
        }
        else
        {
            cloud->points[i].z = 1 - cloud->points[i].x - cloud->points[i].y;
        }

    }

生成的点数据显示效果如下：

1. 定义平面模型对象和RANSAC对象，设定相关参数值。

// 保存局内点索引
    std::vector<int> inliers;
    // 采样一致性模型对象
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
    ransac.setDistanceThreshold(0.01);
    ransac.computeModel();
    ransac.getInliers(inliers);

    std::cout << "局内点：" << inliers.size() << std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
    final->resize(inliers.size());

    pcl::copyPointCloud(*cloud, inliers, *final);
    pcl::io::savePCDFile("2.pcd", *final);

1. 计算平面方程系数

    Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);
    ransac.getModelCoefficients(coef);

已知平面方程的系数d=1，所以将coef[3]的系数归一化为1，则拟合的平面方程系数a=coef[0]/coef[3],b=coef[1]/coef[3]，c=coef[2]/coef[3],d=1
将局内点保存下来看一下效果，发现不满足阈值的外部点全部剔除了