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 pointsoutput.height = 1; // downsampling breaks the organized structureoutput.is_dense = true; // we filter out invalid pointsEigen::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_);elsegetMinMax3D<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 CloudPointfor (std::vector<int>::const_iterator it = indices_->begin(); it != indices_->end(); ++it){if (!input_->is_dense)// Check if the point is invalidif (!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 intervalif ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))continue;}else{// Use a threshold for cutting out points which are too close/far awayif ((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 dataelse{//第一步：遍历所有点并将它们插入到index_vector容器中for (std::vector<int>::const_iterator it = indices_->begin(); it != indices_->end(); ++it){if (!input_->is_dense)// Check if the point is invalidif (!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 sizefirst_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 -1std::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 -1std::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和v2int 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和指定可视化窗口v1viewer.addPointCloud(cloud, "cloud1", v1);viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud1");//设置cloud2的渲染属性，点云的ID和指定可视化窗口v2viewer.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）》的学习。