【PCL】教程extract_indices.cpp 根据索引提取点云

[done, 529.11 ms : 460400 points]
Available dimensions: x y z intensity distance sid

源点云table_scene_lms400.pcd

测试程序

移除预编译器PCL_NO_PRECOMPILE

PointCloud before filtering: 460400 data points.
PointCloud after filtering: 41049 data points.
PointCloud representing the planar component: 20162 data points.
PointCloud representing the planar component: 12125 data points.

[done, 522.573 ms : 41049 points]
Available dimensions: x y z

table_scene_lms400_downsampled.pcd

table_scene_lms400_plane_0.pcd

table_scene_lms400_plane_1.pcd

该段代码是一个C++程序，使用了PCL（点云库）处理3D点云数据。程序执行的主要步骤包括点云的读取、降采样、平面提取，以及连续分割处理。以下是详细的步骤解释：

1. 初始化了几个点云对象：原始点云cloud_blob，过滤后的点云cloud_filtered_blob，经过处理的点云cloud_filtered，以及分割后的点云cloud_p和剩余点云cloud_f。

2. 使用pcl::PCDReader读取名为table_scene_lms400.pcd的PCD文件到原始点云对象中，并输出其点数。

3. 使用pcl::VoxelGrid对原始点云进行降采样，设置叶子格的大小为1cm，得到cloud_filtered_blob。

4. 将过滤后的点云数据从PCLPointCloud2格式转换为普通的点云格式，并输出过滤后的点数。

5. 将降采样后的点云数据写入到磁盘中，文件名为table_scene_lms400_downsampled.pcd。

6. 初始化pcl::ModelCoefficients对象和pcl::PointIndices对象来存储分割算法的结果。

7. 配置平面分割算法，设置模型类型为平面，方法类型为RANSAC，并设置最大迭代次数和距离阈值。

8. 循环执行分割操作直到剩余点云的数量小于原始点云的30%。每一次循环中：

• 使用分割对象seg提取最大的平面组件。

• 如果找不到平面模型，输出错误信息并停止循环。

• 将平面内的点提取到cloud_p中，并输出该平面组件的点数。

• 将提取的平面组件保存到文件中，文件名格式为table_scene_lms400_plane_i.pcd，i为迭代次数。

• 设置提取器extract为负提取模式，然后提取剩余的非平面点云到cloud_f中，并更新cloud_filtered来进行下一轮的迭代分割。

程序结束。

总的来说，此代码主要实现了从一个PCD文件中读取3D点云，通过VOXEL GRID滤波器进行降采样，然后使用随机采样一致性算法（RANSAC）的方法不断分割出最大的平面，直到剩余点云数量低于原始数量的30%。这种方法常用于从复杂的点云场景中提取特定的模型，如地面、桌面等平面。

// 包含标准输入输出流库
#include <iostream>
// 包含PCL库中用于存储模型系数的头文件
#include <pcl/ModelCoefficients.h>
// 包含PCL库中用于PCD文件输入输出的头文件
#include <pcl/io/pcd_io.h>
// 包含PCL库中各种点类型定义的头文件
#include <pcl/point_types.h>
// 包含PCL库中采样一致性算法方法的相关定义的头文件
#include <pcl/sample_consensus/method_types.h>
// 包含PCL库中采样一致性算法模型的相关定义的头文件
#include <pcl/sample_consensus/model_types.h>
// 包含PCL库中平面分割的头文件
#include <pcl/segmentation/sac_segmentation.h>
// 包含PCL库中体素栅格下采样滤波器的头文件
#include <pcl/filters/voxel_grid.h>
// 包含PCL库中根据索引提取点云的滤波器的头文件
#include <pcl/filters/extract_indices.h>


// 主函数入口
int
main ()
{
  // 声明原始点云和滤波后点云的指针
  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
  // 声明模板化点云指针，用于后续处理
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);


  // 填充点云数据
  pcl::PCDReader reader; // 创建PCD文件读取器对象
  reader.read ("table_scene_lms400.pcd", *cloud_blob); // 读取PCD文件


  // 输出滤波前点云数据的数量
  std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;


  // 创建滤波器对象：使用1cm作为叶子大小对数据集进行下采样
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor; // 创建体素栅格下采样滤波器对象
  sor.setInputCloud (cloud_blob); // 设置输入点云
  sor.setLeafSize (0.01f, 0.01f, 0.01f); // 设置体素大小
  sor.filter (*cloud_filtered_blob); // 应用滤波


  // 转换为模板化的点云格式
  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); // 从PCLPointCloud2转换到PointCloud<pcl::PointXYZ>


  // 输出滤波后的点云数据数量
  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;


  // 将下采样后的点云写入硬盘
  pcl::PCDWriter writer; // 创建PCD文件写入器对象
  writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false); // 写入点云到PCD文件


  // 创建模型系数对象和内点索引对象的指针
  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); // 设置距离阈值


  // 创建提取滤波器对象
  pcl::ExtractIndices<pcl::PointXYZ> extract; // 创建根据索引提取点云的滤波器对象


  // 当前点云数量和最初点云数量的初始化
  int i = 0, nr_points = (int) cloud_filtered->size ();
  // 当剩余点云还有30%时继续循环
  while (cloud_filtered->size () > 0.3 * nr_points)
  {
    // 从剩余点云中分割最大平面组件
    seg.setInputCloud (cloud_filtered); // 设置输入点云
    seg.segment (*inliers, *coefficients); // 进行分割操作
    // 如果未找到内点，则输出错误信息并终止循环
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }


    // 提取内点
    extract.setInputCloud (cloud_filtered); // 设置输入点云
    extract.setIndices (inliers); // 设置内点索引
    extract.setNegative (false); // 设置提取内点
    extract.filter (*cloud_p); // 应用滤波并将结果存储到cloud_p
    // 输出平面组件点云的数量
    std::cerr << "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"; // 构造文件名
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); // 写入文件


    // 创建另一个提取滤波器对象用于提取剩余点云
    extract.setNegative (true); // 设置提取除内点之外的其他点
    extract.filter (*cloud_f); // 应用滤波并将结果存储到cloud_f
    cloud_filtered.swap (cloud_f); // 交换滤波后的点云，为下一次迭代准备
    i++; // 计数器递增
  }


  return (0);
}

此代码用于读取PCD文件中的点云数据，并执行以下步骤：

1. 使用体素栅格对点云进行下采样，以减少点的数量；

2. 使用随机采样一致性(RANSAC)算法识别并分割点云中的平面部分；

3. 提取平面组件，并循环执行上述步骤，直到剩余点云的数量降低至原始数量的30%；

4. 将每次分割出的平面组件点云保存为新的PCD文件。

整个过程演示了点云预处理(下采样)、平面分割以及点云分割后处理的常用技术。