PCL(Point Cloud Library)点云降采样方法

PCL点云降采样

降采样简介

本工作中，场景点云通过深度相机测量得到，不同深度相机的点云容量不同，如Photoneo深度相机一次观测（1帧）可得到300万个点，SCAPE深度相机)一次只有3万个点。值得指出，点云容量与点云质量、识别效果没有必然的正比关系，上述两个厂商都是工业界一流的视觉相机厂商、3D视觉方案商。

本工作所使用的深度相机的点云容量约为20万，对人体模型进行观测发现，1帧内人体模型部分的点云偏向稀疏，于是采用多帧融合的方法，得到了稠密点云。但是，这个稠密点云的点密度分布不均匀，会误导ICP算法，导致人体位姿估计陷入局部最优，得到错误的位置和姿态。

为减少因点密度分布不均造成的计算错误，本工作需要使点云的密度在各个空间坐标上分布均匀。为了达成这个目标，采用点云降采样(Downsampling)方法。点云降采样方法的原理是将三维空间网格化，也被称为体素化。网格化后，每一个格子被称为一个体素(Voxel)。降采样的基本思路是检查每一个体素，若体素内有点存在，则用一个点代替体素内的点集。这个点的坐标可以是体素的中心坐标，也可以是点集坐标的期望。

PCL的降采样方法

PCL的降采样方法由类pcl::VoxelGrid<pcl::PCLPointCloud2>实现。需要注意，pcl::PCLPointCloud2是PCL用于保存点云的类，这是一个在PCL早期版本中使用的类。在近期版本中已经被pcl::PointCloud<pcl::PointXYZ> Ptr替代，同样用于保存点云数据，这个类在进行点云的图形显示、空间变换操作等功能时更加方便。

根据上述讨论，为了达到降采样目标，本工作会将点云数据交由pcl::VoxelGrid<pcl::PCLPointCloud2>处理。一旦降采样完成，为了进行后续操作，本工作会将点云数据转换回pcl::PointCloud<pcl::PointXYZ> Ptr。操作流程如下，

• 点云数据读取，采用pcl::PointCloud<pcl::PointXYZ> Ptr对象进行保存；
• 降采样准备工作，将点云数据转换为pcl::VoxelGrid<pcl::PCLPointCloud2>；
• 降采样；
• 降采样结束，将点云数据转换为pcl::PointCloud<pcl::PointXYZ> Ptr；

其中，点云数据读取的工作已经在上文讨论过。

降采样准备工作中，点云数据的转换用到了PCL的函数toPCLPointCloud2，其基本用法如下，

PointCloudT::Ptr cloud_in (new PointCloudT);	// Original point cloud
pcl::PCLPointCloud2::Ptr cloud_in2 (new pcl::PCLPointCloud2 ());
toPCLPointCloud2(*cloud_in,*cloud_in2);

其中，cloud_in是pcl::PointCloud<pcl::PointXYZ> Ptr类型的点云数据，cloud_in2是pcl::VoxelGrid<pcl::PCLPointCloud2>类型的点云数据，从前者到后者的转换采用函数toPCLPointCloud2完成。

降采样方法如下，

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud_in2);
	sor.setLeafSize(0.01f,0.01f,0.01f);
	sor.filter (*cloud_filtered);

其中，pcl::VoxelGrid<pcl::PCLPointCloud2> sor是体素类的对象，待处理的点云cloud_in2通过函数setInputCloud导入体素对象，体素尺寸通过函数setLeafSize(0.01f,0.01f,0.01f)设置，函数filter (*cloud_filtered)设置了降采样结果的保存位置。

降采样结束结束后，为了进行后续操作，需要将点云数据从pcl::VoxelGrid<pcl::PCLPointCloud2>类型转换到pcl::PointCloud<pcl::PointXYZ> Ptr类型。这个操作采用函数fromPCLPointCloud2完成，代码如下，

  fromPCLPointCloud2(*cloud_filtered,*cloud_in);
	cout<<"Scene Point cloud size : "<<cloud_in->points.size()<<endl;

降采样的相关代码如下，

  typedef pcl::PointXYZ PointT;
  typedef pcl::PointCloud<PointT> PointCloudT;

  PointCloudT::Ptr cloud_in (new PointCloudT);	// Original point cloud

  //
	// sparselization of the point cloud
	//
	pcl::PCLPointCloud2::Ptr cloud_in2 (new pcl::PCLPointCloud2 ());
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	toPCLPointCloud2(*cloud_in,*cloud_in2);
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud_in2);
	sor.setLeafSize(0.01f,0.01f,0.01f);
	sor.filter (*cloud_filtered);
	fromPCLPointCloud2(*cloud_filtered,*cloud_in);
	cout<<"Scene Point cloud size : "<<cloud_in->points.size()<<endl;