PCL平面模型分割

#include <iostream>



#include <iostream>
#include <pcl-1.9/pcl/io/pcd_io.h>
#include <pcl-1.9/pcl/point_types.h>
#include <pcl-1.9/pcl/ModelCoefficients.h>
#include <pcl-1.9/pcl/sample_consensus/method_types.h>      // 随机参数估计方法头文件
#include <pcl-1.9/pcl/sample_consensus/model_types.h>       // 模型定义有文件
#include <pcl-1.9/pcl/segmentation/sac_segmentation.h>      // 基于采样一致性分割的类的头文件

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

    // 填充点云数据，设置点云宽度为 15, 高度为 1, 即为无序点云
    cloud->width  = 15;
    cloud->height = 1;
    //cloud->is_dense = false;
    cloud->points.resize (cloud->width * cloud->height);

    // 生成数据，采用随机数填充点云的 x 和 y 坐标, 但都处在 z  =  1 的平面上
    for (auto& point: *cloud)
    {
        point.x = 1024 * rand () / (RAND_MAX + 1.0f);
        point.y = 1024 * rand () / (RAND_MAX + 1.0f);
        point.z = 1.0;
    }

    // 设置几个局外点，即重新设置几个点的 z 值, 使得偏离 z = 1 的平面
    (*cloud)[0].z = 2.0;
    (*cloud)[3].z = -2.0;
    (*cloud)[6].z = 4.0;

    // 保存点云
    pcl::io::savePCDFileASCII("test_pcd.pcd", *cloud);

    // 在标准的输出上打印出点云中各个点的坐标值, 方便分割后的参考
    std::cerr << "Point cloud data: "
              << cloud->size()
              << " points"
              << std::endl;

    for (const auto& point: *cloud)
        std::cerr << " " << point.x
                  << " " << point.y
                  << " " << point.z
                  << std::endl;


    // 创建分割时所需的模型系数对象 coefficients 及存储内点的点索引集合对象 inliers
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // 创建分割对象
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // 可选择配置，设置模型系数需要优化
    seg.setOptimizeCoefficients (true);
    // 必须配置，设置分割模型类型，所用的随机参数估计方法，距离阈值、输入点云
    seg.setModelType (pcl::SACMODEL_PLANE);     // 分割模型类型
    seg.setMethodType (pcl::SAC_RANSAC);              // 参数估计方法
    seg.setDistanceThreshold (0.01);         // 距离阈值

    seg.setInputCloud (cloud);                  // 输入点云
    // 引发分割实现，并存储分割结果到点集合 inliers 及存储平面模型的系数 cofficients
    seg.segment (*inliers, *coefficients);


    if (inliers->indices.size () == 0)
    {
        PCL_ERROR ("Could not estimate a planar model for the given dataset.");
        return (-1);
    }

    // 打印出估计的平面模型参数
    std::cerr << "Model coefficients: "
              << coefficients->values[0] << " "
              << coefficients->values[1] << " "
              << coefficients->values[2] << " "
              << coefficients->values[3] << std::endl;

    // 保存点云

    // 打印出分割了欧宁面之后的点云
    std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
    for (std::size_t i = 0; i < inliers->indices.size (); ++i)
        for (const auto& idx: inliers->indices)
            std::cerr << idx << "    " << cloud->points[idx].x << " "
                      << cloud->points[idx].y << " "
                      << cloud->points[idx].z << std::endl;
     //   pcl::io::savePCDFileASCII("test_pcd_seg.pcd", *cloud);
    return (0);
}

使用 RAN SAC 方法( pcl: : SAC_RANSAC )作为选择的稳健估计方法,距离阔值是 0.01m ,即只要点到 z=1 平面距离小于该阈值的点都作为内点看待,而大于该阁值的则看做外点。