从一个点云中提取索引

//
// Created by xuming on 21-6-7.
//

#include <iostream>
#include <pcl-1.11/pcl/io/pcd_io.h>
#include <pcl-1.11/pcl/point_types.h>
#include <pcl-1.11/pcl/filters/voxel_grid.h>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>


#include <pcl-1.11/pcl/sample_consensus/method_types.h>

#include <pcl-1.11/pcl/ModelCoefficients.h>
#include <pcl-1.11/pcl/segmentation/sac_segmentation.h>

int main()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);


    pcl::PCDReader reader;
    // 把路径改为自己存放的路径

    reader.read("table_scene_lms400.pcd",*cloud);

    std::cout << "PointCloud befor filtering: "
              << cloud->width * cloud->height
              << " data points ("
              << pcl::getFieldsList (*cloud)
              << ")."
              << std::endl;

    // 接下来创建一个 1 cm 的 pcl::VoxelGrid 的滤波器

    pcl::VoxelGrid<pcl::PointXYZ> sor;      // 体素栅格下采样对象
    sor.setInputCloud(cloud);               // 设置下采样原始点云数据
    sor.setLeafSize(0.01f, 0.01f, 0.01f);   // 设置采样的体素大小
    sor.filter(*cloud_filtered);            // 执行采样保存数据 cloud_filtered


    // 处理参数化分割，这里先简单描述

    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.setMaxIterations(1000);                 // 设置最大迭代次数
    seg.setDistanceThreshold(0.01);             // 设置判断是否为模型内点的距离的阈值


    // 设置 extraction filter 的实际参数

    pcl::ExtractIndices<pcl::PointXYZ> extract; // 创建点云提取对象

    int i =0;
    int nr_points = (int) cloud_filtered->points.size();

    while (cloud_filtered->points.size() > 0.3 * nr_points)
    {
        // 为了处理点云中包含多个模型，在一个循环执行过程中，并在每次模型被提取后保存剩余的点，

        seg.setInputCloud(cloud_filtered);
        seg.segment(*inliers, *coefficients);

        if (inliers->indices.size() > 0)
        {
            std::cout << "Could not estimate a planar model for the given dataset. " << std::endl;
        }
    }



    extract.setInputCloud(cloud_filtered);      // 设置输入点云
    extract.setIndices(inliers);                // 设置分割后的内点为需要提取的点集
    extract.setNegative(false);                 // 设置提取内点而非外点
    extract.filter(*cloud_p);                   // 提取输出存储到 cloud_p


    std::cout << "PointCloud representing the planar component: "
              << cloud_p->width * cloud_p->height
              << " data points."
              << std::endl;


    std::stringstream ss;
    ss << "table_scene_lms400_plane_" << i << ".pcd";
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);


    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;







    return 0;
}