PCL的voxelgrid滤波及验证点云强度如何变化

师兄给提了一个小问题：了解PCL的voxelgrid滤波，并验证点云强度怎么变化的。本篇文章围绕此问题研究下。

原理：

引言

  SLAM作用一般分为两种：定位和建图。地图分为很多种，稀疏的，稠密的，半稀疏的，点云图是稀疏的地图，放大了看就是一个个离散的空间点，不过我们可以把它变成连续的稠密的网格，这个过程也叫点云的网格化。点云的网格化需要对点云进行一系列处理，第一步要做的就是点云滤波。

为什么要对点云滤波？

  滤波最早来自数字信号处理里的概念，可以理解成一个过滤器，是对点云的一种预处理方法。可以观察下图，左侧就是原来的点云，右侧是经过滤波后滤掉的“杂质”；

  但是，并不是所有的点云都需要进行滤波处理。一般情况下，以下的几种情况需要进行点云滤波处理：
（1） 点云数据密度不规则需要平滑
（2） 因为遮挡等问题造成离群点需要去除
（3） 大量数据需要下采样
（4） 噪声数据需要去除
  其中点云的噪声有几个方面的来源。一方面来自设备。比如我们用激光扫描仪、RGB-D相机等设备获取点云数据时，由于设备精度，电磁波的衍射特性等都会引入噪声的。另一方面来自环境因素带来的影响，比如被测物体表面性质发生变化。还有一个重要的方面就是操作者经验带来的影响，比如在处理点云数据拼接配准等操作过程中引入的一些噪声等。
  点云中的噪声点对后续操作的影响比较大。如果连实验数据都有较大误差，那也就没有太高的科研价值了。PCL中有一个专门的点云滤波模块，可以将噪声点去除，还可以进行点云压缩等操作，非常灵活实用，例如：双边滤波，统计滤波，条件滤波，随机采样一致性滤波等。这样才能够更好的进行配准，特征提取，曲面重建，可视化等后续应用处理。
  滤波模块主要是调用一些封装好的滤波函数，然后根据需要设定一下参数，还是很直观的。
一般来说，滤波对应的方案有如下几种：
（1）按照给定的规则限制过滤去除点
（2）通过常用滤波算法修改点的部分属性
（3）对数据进行下采样

点云下采样

  为啥要下采样呢？了解原因了就知道什么时候用嘛！我举个例子，点云融合，一张640x480 的Depth图，假如每个地方都有深度值，可以转化为30万个点组成的点云，如果有几十张上百张图这样暴力融合，那这个融合的点云会越来越大，储存、操作都是个大问题！相当于我有个大容量的样本，我按一定的规则从里面抽取有代表性的样本，可以代替原来的样本。体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能，但是会移动点的位置。
  这个下采样PCL中有专门的类，叫做：

class  pcl::ApproximateVoxelGrid< PointT >

下采样的实现原理

  它比较适合对海量的点云在处理前进行数据压缩，而且可以在特征提取等处理中选择合适的体素（voxel）大小等参数，提高算法效率。该函数对输入的点云数据创建一个三维体素栅格，每个体素内用体素中所有点的重心来近似显示体素中其他点，这样该体素内所有点都用一个重心点最终表示。它的优点是可以在下采样的时候保存点云的形状特征。(或者说计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并用该质心的坐标来近似该立方体内的若干点。)
  看下面是它的结果，左边是待处理的右边是处理前，右边是处理后

关键代码就四行：

pcl::VoxelGrid<pcl::PCLPointCloud2> sor;// 创建滤波对象
sor.setInputCloud (cloud);				// 设置需要过滤的点云给滤波对象
sor.setLeafSize (0.01f, 0.01f, 0.01f);	// 设置滤波时创建的体素体积为边长1cm的立方体
sor.filter (*cloud_filtered);			// 执行滤波处理，存储输出

常用的两个函数：

setLeafSize( float lx, float ly, float lz);

setLeafSize后面的三个参数表示体素栅格叶大小，分别表示体素在XYZ方向的尺寸，以米为单位，上面就是设置为长宽高都为1cm

setDownsampleAllData(bool downsample);

设置是否对所有的字段进行下采样。点云有不同的类型，比如有的是 PointXYZ，有的是PointXYZRGB，还有其他类型，也就是一个点包含多种不同信息，比如空间位置XYZ，颜色信息RGB，或者强度信息等，如果想要对所有信息（字段）下采样则设置为true，只对XYZ下采样的话设置为false

如何充分了解每个类的功能?

第一步，登录PCL API documentation
第二步：在右上角搜索框内输入我们要查询的模板类名称，这里我们输入ApproximateVoxelGrid，如下所示，会自动跳出来匹配的结果

第三步：选择你要找的类名，点击进入，就到了下面这个界面，列出了所有的成员变量和成员函数，点击每个成员函数，会跳到对应的解释界面

去除点云的离群点

用到再更咯。

代码实现部分：

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(voxel_grid)

find_package(PCL 1.8 REQUIRED)

include_directories(${
    
    PCL_INCLUDE_DIRS})
link_directories(${
    
    PCL_LIBRARY_DIRS})
add_definitions(${
    
    PCL_DEFINITIONS})
add_executable (voxel_grid voxel_grid.cpp)
target_link_libraries (voxel_grid ${
    
    PCL_LIBRARIES})

滤波后观察位置（x,y,z）信息

voxel_grid.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h>																							//console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
 
int main(int argc, char** argv)
{
    
    
	std::vector<int> filenames;																							//创建一个动态数组用于保存文件名
	filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");										//文件名通过输入来定义
 
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);									//定义共享指针
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
 
	pcl::io::loadPCDFile("table_scene_lms400.pcd", *cloud);
	pcl::visualization::CloudViewer viewer1("Cloud Viewer1");
	viewer1.showCloud(cloud);																						//showcloud函数是同步的，会在此处等到渲染显示
	while (!viewer1.wasStopped())
	{
    
    
 
	}
	std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
 
	pcl::VoxelGrid<pcl::PointXYZI> sor;																					//实例化一个VoxelGrid滤波器
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);																				//设置处理时所使用的体素的大小
	sor.filter(*cloud_filter);
	std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
 
	pcl::io::savePCDFileASCII("output_downsampled", *cloud_filter);
	pcl::visualization::CloudViewer viewer2("Cloud Viewer2");
	viewer2.showCloud(cloud_filter);																						//showcloud函数是同步的，会在此处等到渲染显示
	while (!viewer2.wasStopped())
	{
    
    
 
	}
	return(0);
}

运行结果：

angle1：


angle2：


可以看到在立方体边长为1cm时点云数量近似为原来的0.1倍，下采样效果明显。LeafSize设置的越大，体素格子也就越大，滤波效果也就越明显：

sor.setLeafSize(0.02f, 0.02f, 0.02f);

当LeafSize为0.02时，结果：



可以明显观察到滤波后的点的密度明显比刚才更小了，实际输出的数据也表明只有原来的$\frac{1}{40}$。

滤波后观察强度（indensity）信息

  本来按理说这个强度应该也是和位置一样是取质心的，但是位置是可以明显的从图中观察出来，但是强度这个值在图中并不明显。所以我继续采用自定义点云中的点来测试。

保存测试数据为PCD格式

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

int main(int argc, char** argv)
{
    
    
    pcl::PointCloud<pcl::PointXYZI> cloud;

    // Fill in the cloud data
    cloud.width = 3;            //点云个数
    cloud.height = 1;           //无序点云
    cloud.is_dense = false;     //不是密集点云
    cloud.points.resize(cloud.width * cloud.height);    //确定点云的大小

    //定义3个点，并存入cloud中
	cloud.points[0].x = 0.1;
	cloud.points[0].y = 0.1;
	cloud.points[0].z = 0.1;
	cloud.points[0].intensity = 0.1;

	cloud.points[1].x = 0.2;
	cloud.points[1].y = 0.2;
	cloud.points[1].z = 0.2;
	cloud.points[1].intensity = 0.2;

	cloud.points[2].x = 0.3;
	cloud.points[2].y = 0.3;
	cloud.points[2].z = 0.3;
	cloud.points[2].intensity = 0.3;
    //将cloud中的点云数据保存至test_pcd.pcd文件中
    pcl::io::savePCDFileASCII("test_pcd.pcd", cloud);
    std::cerr << "Saved " << cloud.points.size() << " data points to test_pcd.pcd." << std::endl;

    return (0);
}

读取PCD格式数据并输出

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

int main(int argc, char** argv)
{
    
    
    //创建一个保存点云的对象
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    //从文件中加载点云
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("test_pcd.pcd", *cloud) == -1) //* load the file
    {
    
    
        PCL_ERROR("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }
    std::cout << "Loaded "
        << cloud->width * cloud->height
        << " data points from test_pcd.pcd with the following fields: "
        << std::endl;
    //打印点云
    for (std::size_t i = 0; i < cloud->points.size(); ++i)
        std::cout << "    " << cloud->points[i].x
        << " " << cloud->points[i].y
        << " " << cloud->points[i].z
		<< " " << cloud->points[i].intensity  << std::endl;

    return (0);
}

小数量测试数据代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h>																							//console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
 
int main(int argc, char** argv)
{
    
    
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);									//定义共享指针
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
 
	pcl::io::loadPCDFile("test_pcd.pcd", *cloud);
	std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
 
	pcl::VoxelGrid<pcl::PointXYZI> sor;																					//实例化一个VoxelGrid滤波器
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.4, 0.4, 0.4);																				//设置处理时所使用的体素的大小
	sor.filter(*cloud_filter);
	std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
	pcl::io::savePCDFileASCII("test_after.pcd", *cloud_filter);
	return(0);
}

初步结论如下:

  在修改以上少数数据多次测试过后中我发现，体素滤波器选择的立方体应当是从（0，0，0）坐标开始的，本例中我采用的是叶子尺寸为(0.4,0.4,0.4)（单位为M），则实际上体素滤波后的点云坐标就是位于x:[0,0.4)，y:[0,0.4)，z:[0,0.4)中的全部点的质心(注意：是包含0但是不包含0.4，负数以及后续点的值应当遵守这一规律，如[-0.4,0))。且质心的坐标计算十分朴实，即：
$$(x^{{}^{\prime }},y^{{}^{\prime }},z^{{}^{\prime }},I^{{}^{\prime }})=\frac{\sum _{0⩽i＜n}(x_i,y_i,z_i,I_i)}{n}，其中n为范围内的点的个数$$
当然强度则是跟随着点一起。

中等数据量测试上述结论:

测试代码如下

input.cpp

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

int main(int argc, char** argv)
{
    
    
    srand(time(NULL));      //随机数种子
    pcl::PointCloud<pcl::PointXYZI> cloud;

    // Fill in the cloud data
    cloud.width = 100;            //点云个数
    cloud.height = 1;           //无序点云
    cloud.is_dense = false;     //不是密集点云
    cloud.points.resize(cloud.width * cloud.height);    //确定点云的大小
    double c[8][4] = {
    
    {
    
    0,0,0,0}, {
    
    0,0,0,0},{
    
    0,0,0,0} ,{
    
    0,0,0,0},{
    
    0,0,0,0}, {
    
    0,0,0,0},{
    
    0,0,0,0} ,{
    
    0,0,0,0} };  
    //打算取leafsize为0.5，即将大小为1的立方体分割为8块(即第一个纬度为8)，所以应当有8个质心（点），第二个纬度分别代表(x,y,z,I)
    int n[8] = {
    
    0,0,0,0,0,0,0,0};  //记录方块内的数据个数
    

    //生成100个0~1的随机数，并存入cloud中
    for (std::size_t i = 0; i < cloud.points.size(); ++i)
    {
    
    
        cloud.points[i].x = rand() / (RAND_MAX + 1.0f);  //随即生成[0,1)
        cloud.points[i].y = rand() / (RAND_MAX + 1.0f);
        cloud.points[i].z = rand() / (RAND_MAX + 1.0f);
        cloud.points[i].intensity = rand() / (RAND_MAX + 1.0f);
        if(cloud.points[i].x >= 0 &&  cloud.points[i].x < 0.5 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
        {
    
    
            c[0][0] += cloud.points[i].x;
            c[0][1] += cloud.points[i].y;
            c[0][2] += cloud.points[i].z;
            c[0][3] += cloud.points[i].intensity;
            n[0]++;
        }
        else if(cloud.points[i].x >= 0 &&  cloud.points[i].x < 0.5 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
        {
    
    
            c[1][0] += cloud.points[i].x;
            c[1][1] += cloud.points[i].y;
            c[1][2] += cloud.points[i].z;
            c[1][3] += cloud.points[i].intensity;
            n[1]++;
        }
        else if(cloud.points[i].x >= 0 &&  cloud.points[i].x < 0.5 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
        {
    
    
            c[2][0] += cloud.points[i].x;
            c[2][1] += cloud.points[i].y;
            c[2][2] += cloud.points[i].z;
            c[2][3] += cloud.points[i].intensity;
            n[2]++;
        }
        else if(cloud.points[i].x >= 0 &&  cloud.points[i].x < 0.5 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
        {
    
    
            c[3][0] += cloud.points[i].x;
            c[3][1] += cloud.points[i].y;
            c[3][2] += cloud.points[i].z;
            c[3][3] += cloud.points[i].intensity;
            n[3]++;
        }
        else if(cloud.points[i].x >= 0.5 &&  cloud.points[i].x < 1 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
        {
    
    
            c[4][0] += cloud.points[i].x;
            c[4][1] += cloud.points[i].y;
            c[4][2] += cloud.points[i].z;
            c[4][3] += cloud.points[i].intensity;
            n[4]++;
        }
        else if(cloud.points[i].x >= 0.5 &&  cloud.points[i].x < 1 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
        {
    
    
            c[5][0] += cloud.points[i].x;
            c[5][1] += cloud.points[i].y;
            c[5][2] += cloud.points[i].z;
            c[5][3] += cloud.points[i].intensity;
            n[5]++;
        }
        else if(cloud.points[i].x >= 0.5 &&  cloud.points[i].x < 1 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
        {
    
    
            c[6][0] += cloud.points[i].x;
            c[6][1] += cloud.points[i].y;
            c[6][2] += cloud.points[i].z;
            c[6][3] += cloud.points[i].intensity;
            n[6]++;
        }
        else if(cloud.points[i].x >= 0.5 &&  cloud.points[i].x < 1 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
        {
    
    
            c[7][0] += cloud.points[i].x;
            c[7][1] += cloud.points[i].y;
            c[7][2] += cloud.points[i].z;
            c[7][3] += cloud.points[i].intensity;
            n[7]++;
        }
    }
    for(int i = 0;i < 8; i++)
        for(int j = 0;j < 4; j++)
            c[i][j] = c[i][j] / n[i];  //将前面的范围内的点求质心
     for(int i = 0;i < 8; i++)
    {
    
       
        std::cout << "第"<< i + 1 << "个块有" << n[i] << "个数"<< std::endl;
        std::cout << "第"<< i + 1 << "个点为" <<"("<< c[i][0]<<","<< c[i][1] <<  "," << c[i][2] << "," << c[i][3] << ")" << std::endl;
    }

    //将cloud中的点云数据保存至test_pcd.pcd文件中
    pcl::io::savePCDFileASCII("test_pcd.pcd", cloud);
    std::cerr << "Saved " << cloud.points.size() << " data points to test_pcd.pcd." << std::endl;

    return (0);
}

test.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h>																							//console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
 
int main(int argc, char** argv)
{
    
    
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);									//定义共享指针
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
 
	pcl::io::loadPCDFile("test_pcd.pcd", *cloud);
	std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
 
	pcl::VoxelGrid<pcl::PointXYZI> sor;																					//实例化一个VoxelGrid滤波器
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.5, 0.5, 0.5);																				//设置处理时所使用的体素的大小
	sor.filter(*cloud_filter);
	std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
	pcl::io::savePCDFileASCII("test_after.pcd", *cloud_filter);
	return(0);
}

test_after.pcd

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 8
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 8
DATA ascii
0.13851911 0.24136128 0.33553141 0.40700749
0.64431816 0.24781057 0.24998038 0.53771544
0.29668111 0.72371972 0.30309331 0.58366519
0.76265746 0.68379813 0.26003802 0.55480695
0.30543846 0.25518233 0.69257158 0.61186677
0.75259429 0.23724338 0.78009278 0.4655337
0.1827049 0.72605532 0.72325444 0.63117778
0.76790327 0.79831183 0.74001992 0.52766722

  上述结果可以验证确实满足初步结论的。并且还可以观察到，点云中的数据排列是按照$x,y,z$依次增大的顺序输出滤波后的点的，而笔者采用的是$z,y,x$依次增大的顺序输出，所以略有出入。边界点比较简单，笔者在此就不予展示了，读者只需要自行设置一两个点验证即可。

参考资料：

Reference
从零开始一起学习SLAM | 给点云加个滤网
PCL教程-点云滤波之体素滤波器（下采样）
PCL点云库调库学习系列——点云数据输入与输出
在ubuntu上安装pcl库并配置vscode使用cmake生成可执行文件