PCL教程-点云滤波之体素滤波器（下采样）

原文链接：Downsampling a PointCloud using a VoxelGrid filter

点云文件下载： table_scene_lms400.pcd

目录

原理

程序代码

PCLPointCloud2

 实验结果

---

在本篇教程中，我们将使用体素化的方式对点云数据进行下采样。

原理

下采样：减少点云数据集中点云数量

体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能，但是会移动点的位置。 此外体素滤波器可以去除一定程度的噪音点及离群点。主要功能是用来进行降采样。
它的原理是根据输入的点云，首先计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并用该质心的坐标来近似该立方体内的若干点。

ApproximateVoxelGrid的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于 VoxelGrid，计算速度稍快，但也损失了原始点云局部形态的精细度。

使用VoxelGrid类在输入点云数据上创建一个3D体素网格(将体素网格看作一组空间中的微小3D盒子)。然后，在每个体素(3D盒)中，所有存在的点将以其质心近似(即下采样)取代。这种方法比用体素的中心来近似它们要慢一些，但它更准确地表示点云表面。

程序代码

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

int
main (int argc, char** argv)
{
  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!

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

  // Create the filtering object
  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;

  pcl::PCDWriter writer;
  writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

  return (0);
}

原文代码中使用了PCLPointCloud2 类来保存点云。

下面简单了解下PCLPointCloud2类

PCLPointCloud2

pcl::PCLPointCloud2是一个ROS(机器人操作系统)消息类型，取代了旧的sensors_msgs::PointCloud2。因此，它只能在与ROS交互时使用。

如果需要PCL提供了两个函数进行PCLPointCloud2和pcl::PointCloud之间的转换：

// 将PCLPointCloud2 转换成 PointCloud 
void fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, pcl::PointCloud<PointT>& cloud);
// 将PointCloud 转换成 PCLPointCloud2
void toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg);

PCLPointCloud2的结构定义： 

struct PCLPointCloud2
 {
  PCLPointCloud2 () : header (), height (0), width (0), fields (),
 is_bigendian (false), point_step (0), row_step (0),
  data (), is_dense (false)
  {
 #if defined(BOOST_BIG_ENDIAN)
  is_bigendian = true;
 #elif defined(BOOST_LITTLE_ENDIAN)
  is_bigendian = false;
 #else
 #error "unable to determine system endianness"
 #endif
  }
 
  ::pcl::PCLHeader header;
 
  pcl::uint32_t height;
  pcl::uint32_t width;
 
  std::vector< ::pcl::PCLPointField> fields;
 
  pcl::uint8_t is_bigendian;
  pcl::uint32_t point_step;
  pcl::uint32_t row_step;
 
  std::vector<pcl::uint8_t> data;
 
  pcl::uint8_t is_dense;
 
  public:
  typedef boost::shared_ptr< ::pcl::PCLPointCloud2> Ptr;
  typedef boost::shared_ptr< ::pcl::PCLPointCloud2 const> ConstPtr;
  }; // struct PCLPointCloud

由于PCLVisualizer类支持显示的点云类型为pcl::PointCloud<PointT>，所以无法直接对PCLPointCloud2类型点云进行显示。

 所以有两种解决办法：

办法1：直接使用pcl::PointCloud<PointT>点云类型

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include<pcl/visualization/pcl_visualizer.h>
int
main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::visualization::PCLVisualizer viewer("voxel_grid_filter");
	int v1(1);
	int v2(2);
	viewer.createViewPort(0, 0, 0.5, 1,v1);
	viewer.createViewPort(0.5, 0, 1, 1,v2);

	// Fill in the cloud data
	pcl::PCDReader reader;
	// Replace the path below with the path where you saved your file
	reader.read("table_scene_lms400.pcd", *cloud); // Remember to download the file first!

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

	// Create the filtering object
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);//设置滤波时创建的体素体积为1 cm3的立方体
	sor.filter(*cloud_filtered);

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

	pcl::PCDWriter writer;
	writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered);

	viewer.addPointCloud(cloud, "cloud", v1);
	viewer.addPointCloud(cloud_filtered, "cloud_filtered", v2);

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return (0);
}

办法2：将 PCLPointCloud2转缓存pcl::PointCloud类型

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2(*cloud_filtered, *cloud_filtered2);
	pcl::fromPCLPointCloud2(*cloud, *cloud2);

 完整代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include<pcl/visualization/pcl_visualizer.h>
int
main(int argc, char** argv)
{
	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());
	pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());
	pcl::visualization::PCLVisualizer viewer("voxel_grid_filter");
	int v1(1);
	int v2(2);
	viewer.createViewPort(0, 0, 0.5, 1,v1);
	viewer.createViewPort(0.5, 0, 1, 1,v2);

	// Fill in the cloud data
	pcl::PCDReader reader;
	// Replace the path below with the path where you saved your file
	reader.read("table_scene_lms400.pcd", *cloud); // Remember to download the file first!

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

	// Create the filtering object
	//pcl::VoxelGrid<pcl::PointXYZ> sor;
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);//设置滤波时创建的体素体积为1 cm3的立方体
	sor.filter(*cloud_filtered);

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

	pcl::PCDWriter writer;
	//writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered);
	writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered,
		Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);
	

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2(*cloud_filtered, *cloud_filtered2);
	pcl::fromPCLPointCloud2(*cloud, *cloud2);

	viewer.addPointCloud(cloud2, "cloud", v1);
	viewer.addPointCloud(cloud_filtered2, "cloud_filtered", v2);

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return (0);
}

 实验结果

右图为原始点云，左图为经过体素滤波之后的点云。

 可以看到点云数量近似为原来的0.1倍，下采样效果明显：

PointCloud before filtering: 460400 data points (x y z).
PointCloud after filtering: 41049 data points (x y z). 

 LeafSize设置的越大，体素格子也就越大，滤波效果也就越明显：

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

当LeafSize为0.02时，结果：

PointCloud before filtering: 460400 data points (x y z intensity distance sid).
PointCloud after filtering: 11598 data points (x y z intensity distance sid).