PCL对大规模点云进行体素降采样

普通体素降采样

直接调用voxel_grid库，参考官方教程，只需要设置体素的大小setLeafSize(x, y, z)。

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

int main ()
{
    pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
    pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

    // 读入点云
    pcl::PCDReader reader;
    reader.read ("table_scene_lms400.pcd", *cloud); 
    std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height  << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
    
    // 过滤
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloud); // 输入点云
    sor.setLeafSize (0.01f, 0.01f, 0.01f); // 设置体素大小
    sor.filter (*cloud_filtered);

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;

    return (0);
}

然而，对于大规模的点云（如点个数超过100000），就会导致格点数超过整型最大值，报错：

Leaf size is too small for the input dataset. Integer indices would overflow.

此时可以采用八叉树结构，将整个点云分成有大小限制的若干个叶节点，对每个叶节点分别做体素过滤，再合起来

octree体素过滤

// 原大规模点云
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>());

// 输出的降采样点云
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZI>());

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp_ds(new pcl::PointCloud<pcl::PointXYZI>());

// 同样的体素过滤器VoxelGrid
static float kLeafSize = 0.2;
pcl::VoxelGrid<pcl::PointXYZI> filter_map;
filter_map.setLeafSize(kLeafSize, kLeafSize, kLeafSize);

// 将点云存入octree,设置子区域大小为1250*leaf_size（1250^3<2^31，这样点数不会超过整型范围）

cloud_out->clear();
pcl::octree::OctreePointCloud<pcl::PointXYZI> octree{1250 * kLeafSize};
octree.setInputCloud(cloud_map);
octree.addPointsFromInputCloud();

// 遍历每个区域降采样
for (auto it = octree.leaf_depth_begin(); it != octree.leaf_depth_end(); ++it) {
  auto ids = it.getLeafContainer().getPointIndicesVector();
  cloud_tmp->clear();
  for (auto id : ids) {
    cloud_tmp->push_back(octree.getInputCloud()->points[id]);
  }
  filter_map.setInputCloud(cloud_tmp);
  filter_map.filter(*cloud_tmp_ds);
  *cloud_out += *cloud_tmp_ds;
}