PCL点云库入门——PCL库点云滤波算法之体素滤波（VoxelGrid）

1、体素滤波算法原理

        体素滤波VoxelGrid算法是一种专门用于三维点云数据简化的有效方法。其核心思想是通过将三维空间划分为一系列等大的立方体单元，即体素（Voxel），来减少点云数据的密度。具体来说，算法首先设定一个体素的大小，这个大小决定了数据简化的程度。体素越大，保留的点云数据越少，数据简化程度越高；反之，体素越小，保留的点云数据越多，数据简化程度越低。然后，算法将原始的点云数据映射到这些体素中。对于每个体素，算法计算其内部所有点的质心（或平均位置），并将该质心点作为该体素的代表点。这样，原始的点云数据就被压缩为一系列体素的代表点，从而实现了数据的简化。具体的执行步骤如下：

        第1步：根据输入的点云数据,获取在X、Y、Z三个方向上的边界值Xmax、Ymax、Zmax和Xmin、Ymin、Zmin。

        第2步：根据在X、Y、Z坐标轴上的最大值和最小值，计算出点云数据的AABB包围盒边长Lx、Ly和Lz。

        第3步：根据设置体素的边长Cx、Cy、Cz，将点云数据在X、Y、Z坐标轴平均分层M、N、O分，则点云数据被分为M*N*O个体素，Vnum=M*N*O。

其中，表示向下取整，Vnum为总的体素个数。 

        第4步：计算每个点云数据所属那个体素，体素的三维索引号为（i,j,k），其中：

        为了方便算法的后序计算，常将体素的三维索引号转换为的成一维 索引Idx（类似于HashMap的操作）:

Idx=i*A+j*B+k*C

其中：i,j,k为三维索引号，A、B、C设置的值，具体看情况而设定。

        第5步：对每个体素内包含的点云数据进行处理，计算每个体素点云数据的重心Gijk,以重心代替该体素内的所有点;如重心点不存在,则用体素内距离所求重心最近的数据点代替该体素内的所有点,至此完成整个滤波算法过程。其中Gijk计算式子为：

2、主要成员变量和函数

        1、主要成员变量

        1）、 划分体素的尺寸大小

      Eigen::Vector4f leaf_size_;
    
      //出于效率考虑，存储体素大小的倒数
      Eigen::Array4f inverse_leaf_size_;

        2）、是否所有字段进行下采样，则设置为true，如果只是XYZ，则设置为false

      bool downsample_all_data_;

        3）、是否要将体素局部信息需要保存在 leaf_layout中，则要则设置为true。

      bool save_leaf_layout_;

        4）、保存体素局部信息，用于在当前位置快速访问领域信息

       std::vector<int> leaf_layout_;

        5）、 滤波名字

        std::string filter_field_name_;

        6）、 最小、大允许的过滤值

      double filter_limit_min_;
   
      //最大允许的过滤值
      double filter_limit_max_;

        7）、如果我们想要滤波相反的数据结果（filter_limit_min_;  filter_limit_max_），则设置为true。默认值:false

      bool filter_limit_negative_;

        8）、 计算时质心每个体素钟的最小的点云数目

      unsigned int min_points_per_voxel_;

        9)、将三维体素索引转换为一维索引需要的参数，其中min_b_, max_b_为划分位置的起始（包含x,y,z）方向，div_b_为划分的数，对应上面原理中的M、N、O，divb_mul为上面原理中A、B、C值，其中：A=1；B=M；C=M*N。

      Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;

      2、主要成员函数

       1）、设置体素网格大小

     inline void setLeafSize (float lx, float ly, float lz)
   
     inline void setLeafSize (const Eigen::Vector4f &leaf_size)

3、主要实现代码注解

template <typename PointT> void
pcl::VoxelGrid<PointT>::applyFilter(PointCloud& output)
{
       // Has the input dataset been set already?
       if (!input_)
       {
              PCL_WARN("[pcl::%s::applyFilter] No input dataset given!\n",  getClassName().c_str());
              output.width = output.height = 0;
              output.points.clear();
              return;
       }
       // Copy the header (and thus the frame_id) + allocate enough space for points
       output.height = 1;                    // downsampling breaks the organized  structure
       output.is_dense = true;                 // we filter out invalid points
       Eigen::Vector4f min_p, max_p;
       // 获取点云数据的在三个坐标轴上的最小（Xmin,Ymin,Zmin）最大值(Xmax,Ymax,Zmax)
       if (!filter_field_name_.empty()) // If we don't want to process the entire  cloud...
              getMinMax3D<PointT>(input_, *indices_, filter_field_name_,  static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p,  max_p, filter_limit_negative_);
       else
              getMinMax3D<PointT>(*input_, *indices_, min_p, max_p);
       // 在给定体素尺寸大小的情况下，检查设置的参数是否太小
       std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) *  inverse_leaf_size_[0]) + 1;
       std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) *  inverse_leaf_size_[1]) + 1;
       std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) *  inverse_leaf_size_[2]) + 1;
       if ((dx * dy * dz) >  static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
       {
              PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input  dataset. Integer indices would overflow.", getClassName().c_str());
              output = *input_;
              return;
       }
       // 计算最小和最大边界框值，向下取整
       min_b_[0] = static_cast<int> (std::floor(min_p[0] * inverse_leaf_size_[0]));
       max_b_[0] = static_cast<int> (std::floor(max_p[0] * inverse_leaf_size_[0]));
       min_b_[1] = static_cast<int> (std::floor(min_p[1] * inverse_leaf_size_[1]));
       max_b_[1] = static_cast<int> (std::floor(max_p[1] * inverse_leaf_size_[1]));
       min_b_[2] = static_cast<int> (std::floor(min_p[2] * inverse_leaf_size_[2]));
       max_b_[2] = static_cast<int> (std::floor(max_p[2] * inverse_leaf_size_[2]));
       // 计算沿所有轴所需的划分数，如上公式中的（M、N、O）
       div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
       div_b_[3] = 0;
       // 计算体素1维idx所需要的参数
       divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);
       // 用于点和所属体素的存储
       std::vector<cloud_point_index_idx> index_vector;
       index_vector.reserve(indices_->size());
       // 如果不想处理整个云，而是先过滤远离视点的点，相当于预处理
       if (!filter_field_name_.empty())
       {
              // 获取滤波的字段
              std::vector<pcl::PCLPointField> fields;
              int distance_idx = pcl::getFieldIndex<PointT>(filter_field_name_,  fields);
              if (distance_idx == -1)
                      PCL_WARN("[pcl::%s::applyFilter] Invalid filter field name.  Index is %d.\n", getClassName().c_str(), distance_idx);
              // First pass: go over all points and insert them into the index_vector  vector
              // with calculated idx. Points with the same idx value will contribute  to the
              // same point of resulting CloudPoint
              for (std::vector<int>::const_iterator it = indices_->begin(); it !=  indices_->end(); ++it)
              {
                      if (!input_->is_dense)
                             // Check if the point is invalid
                             if (!std::isfinite(input_->points[*it].x) ||
                                    !std::isfinite(input_->points[*it].y) ||
                                    !std::isfinite(input_->points[*it].z))
                                    continue;
                      // 获取字段的值
                      const std::uint8_t* pt_data = reinterpret_cast<const  std::uint8_t*> (&input_->points[*it]);
                      float distance_value = 0;
                      memcpy(&distance_value, pt_data + fields[distance_idx].offset,  sizeof(float));
                      //对字段的值根据设置的限制进行滤波，类似于直通滤波操作
                      if (filter_limit_negative_)
                      {
                             // Use a threshold for cutting out points which inside  the interval
                             if ((distance_value < filter_limit_max_) &&  (distance_value > filter_limit_min_))
                                    continue;
                      }
                      else
                      {
                             // Use a threshold for cutting out points which are too  close/far away
                             if ((distance_value > filter_limit_max_) ||  (distance_value < filter_limit_min_))
                                    continue;
                      }
                      //计算点所属体素的三维ID号（i,j,l）
                      int ijk0 = static_cast<int> (std::floor(input_->points[*it].x *  inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
                      int ijk1 = static_cast<int> (std::floor(input_->points[*it].y *  inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
                      int ijk2 = static_cast<int> (std::floor(input_->points[*it].z *  inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
                      // 计算点所属体素的1维ID号（idx）,并将点云和所属体素的1维idx保存起来
                      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 *  divb_mul_[2];
                      index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
              }
       }
       // No distance filtering, process all data
       else
       {
       //第一步：遍历所有点并将它们插入到index_vector容器中
              for (std::vector<int>::const_iterator it = indices_->begin(); it !=  indices_->end(); ++it)
              {
                      if (!input_->is_dense)
                             // Check if the point is invalid
                             if (!std::isfinite(input_->points[*it].x) ||
                                    !std::isfinite(input_->points[*it].y) ||
                                    !std::isfinite(input_->points[*it].z))
                                    continue;
                      //计算点所属体素的三维ID号（i,j,l）
                      int ijk0 = static_cast<int> (std::floor(input_->points[*it].x *  inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
                      int ijk1 = static_cast<int> (std::floor(input_->points[*it].y *  inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
                      int ijk2 = static_cast<int> (std::floor(input_->points[*it].z *  inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
                      // 计算点所属体素的1维ID号（idx）,并将点云和所属体素的1维idx保存起来
                      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 *  divb_mul_[2];
                      index_vector.emplace_back(static_cast<unsigned int> (idx), *it);
              }
       }
       //第二步：对index_vector容器进行排序，具体相同信息的所有点将彼此相邻，因为idx相近
       std::sort(index_vector.begin(), index_vector.end(),  std::less<cloud_point_index_idx>());
       // 第三步: 计算输出体素信息
       // 要跳过所有体素idx相同的
       unsigned int total = 0;
       unsigned int index = 0;
       // first_and_last_indices_vector[i] represents the index in index_vector of the  first point in
       // index_vector belonging to the voxel which corresponds to the i-th output  point,
       // and of the first point not belonging to.
       std::vector<std::pair<unsigned int, unsigned int> >  first_and_last_indices_vector;
       // Worst case size
       first_and_last_indices_vector.reserve(index_vector.size());
       while (index < index_vector.size())
       {
              unsigned int i = index + 1;
              //跳过体素idx相同的
              while (i < index_vector.size() && index_vector[i].idx ==  index_vector[index].idx)
                      ++i;
              
              if (i - index >= min_points_per_voxel_)
              {
                      ++total;
                      //将有相同体素idx的点云索引起点和终点保存起来
                      first_and_last_indices_vector.emplace_back(index, i);
              }
              index = i;
       }
       // 第四步：计算质心，将其插入最终位置
       output.points.resize(total);
       if (save_leaf_layout_)
       {
              //保存邻域信息初始化
              try
              {
                      // Resizing won't reset old elements to -1.  If leaf_layout_ has  been used previously, it needs to be re-initialized to -1
                      std::uint32_t new_layout_size = div_b_[0] * div_b_[1] *  div_b_[2];
                      //This is the number of elements that need to be re-initialized  to -1
                      std::uint32_t reinit_size = std::min(static_cast<unsigned int>  (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
                      for (std::uint32_t i = 0; i < reinit_size; i++)
                      {
                             leaf_layout_[i] = -1;
                      }
                      leaf_layout_.resize(new_layout_size, -1);
              }
              catch (std::bad_alloc&)
              {
                      throw PCLException("VoxelGrid bin size is too low; impossible to  allocate memory for layout",
                             "voxel_grid.hpp", "applyFilter");
              }
              catch (std::length_error&)
              {
                      throw PCLException("VoxelGrid bin size is too low; impossible to  allocate memory for layout",
                             "voxel_grid.hpp", "applyFilter");
              }
       }
       index = 0;
       for (const auto& cp : first_and_last_indices_vector)
       {
              // 计算输入点的质心值，在index_vector数组中具有相同的idx值
              unsigned int first_index = cp.first;
              unsigned int last_index = cp.second;
              // 保存质心的索引
              if (save_leaf_layout_)
                      leaf_layout_[index_vector[first_index].idx] = index;
              //非全采样
              if (!downsample_all_data_)
              {
                      // 计算值质心
                      Eigen::Vector4f centroid(Eigen::Vector4f::Zero());
                      for (unsigned int li = first_index; li < last_index; ++li)
                             centroid +=  input_->points[index_vector[li].cloud_point_index].getVector4fMap();
                      centroid /= static_cast<float> (last_index - first_index);
                      output.points[index].getVector4fMap() = centroid;
                      
              }
              else
              {
                      CentroidPoint<PointT> centroid;
                      // 计算值质心
                      for (unsigned int li = first_index; li < last_index; ++li)
                             centroid.add(input_->points[index_vector[li].cloud_point_index]);
                      //将质心结果保存到输出数据中
                      centroid.get(output.points[index]);
              }
              ++index;
       }
       output.width = static_cast<std::uint32_t> (output.points.size());
}

4、代码示例

        新建文件PCLVoxelGridmain.cpp，内容如下：

/*****************************************************************//**
* \file   PCLVoxelGridmain.cpp
* \brief
*
* \author YZS
* \date   December 2024
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
using namespace std;


void PCLVoxelGrid()
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new  pcl::PointCloud<pcl::PointXYZRGB>());
	std::string fileName = "E:/PCLlearnData/10/fragment.pcd";
	pcl::io::load(fileName, *cloud);
	std::cout << "Cloud Size:" << cloud->points.size() << std::endl;
	//保存滤波后的结果
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFilter(new  pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::VoxelGrid<pcl::PointXYZRGB> voxelFilter;// 体素滤波器类对象
	voxelFilter.setInputCloud(cloud);
	voxelFilter.setLeafSize(0.05f, 0.05f, 0.05f);//设置体素尺寸大小
	voxelFilter.filter(*cloudFilter); // 执行滤波，并且保存结果到cloudFilter中

	std::cout << "filter Cloud Size:" << cloudFilter->points.size() << std::endl;
	//结果可视化
// PCLVisualizer对象
	pcl::visualization::PCLVisualizer viewer("FilterVIS");
	//创建左右窗口的ID v1和v2
	int v1(0);
	int v2(1);
	//设置V1窗口尺寸和背景颜色
	viewer.createViewPort(0.0, 0.0, 0.5, 1, v1);
	viewer.setBackgroundColor(0, 0, 0, v1);
	//设置V2窗口尺寸和背景颜色
	viewer.createViewPort(0.5, 0.0, 1, 1, v2);
	viewer.setBackgroundColor(0.1, 0.1, 0.1, v2);
	// 添加2d文字标签
	viewer.addText("v1", 10, 10, 20, 1, 0, 0, "Txtv1", v1);
	viewer.addText("v2", 10, 10, 20, 0, 1, 0, "Txtv2", v2);
	//设置cloud1的渲染属性，点云的ID和指定可视化窗口v1
	viewer.addPointCloud(cloud, "cloud1", v1);
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud1");
	//设置cloud2的渲染属性，点云的ID和指定可视化窗口v2
	viewer.addPointCloud(cloudFilter, "cloud2", v2);
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud2");
	// 可视化循环主体
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
}
int main(int argc, char* argv[])
{
	PCLVoxelGrid();
	std::cout << "Hello World!" << std::endl;
	std::system("pause");
	return 0;
}

        结果：

        注意：由于使用质心近似替代原始点，可能会导致点的位置发生微小移动，这可能会影响某些应用中对位置精度要求较高的场景。

        至此完成第十节PCL库点云滤波算法之体素滤波（VoxelGrid）学习，下一节我们将进入《PCL库中点云滤波之半径滤波（RadiusOutlierRemoval）》的学习。