ROS::点云PCL(4)降采样

理解

通过体素网格实现降采样，可以减少点数量的同时，保证点云的形状特征，可以提高配准、曲面重建、形状识别等算法的速度，并保证准确性。

先来看看降采前的点云

#include  
#include  
#include  

#include 
#include 

#include 
#include 
#include 
#include 


main (int argc, char **argv) 
{ 

    ros::init (argc, argv, "pcl_create"); 

    ros::NodeHandle nh; 
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);  
	
	pcl::PCLPointCloud2 cloud;  
	sensor_msgs::PointCloud2 output;

	pcl::io::loadPCDFile("/home/zjh/cloud_test/table_scene_lms400.pcd",cloud);
	
	pcl_conversions::moveFromPCL(cloud, output); 

    output.header.frame_id = "odom"; 
    ros::Rate loop_rate(1); 

    while (ros::ok()) { 
		pcl_pub.publish(output);
		ros::spinOnce(); 
		loop_rate.sleep(); 
    } 

    return 0;
}

开始降采样

#include 
#include "boost/range.hpp"
#include  
#include  
#include  
#include  

#include 
#include 

#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

main (int argc, char **argv) 
{ 

    ros::init (argc, argv, "pcl_create"); 

    ros::NodeHandle nh; 
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);  

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);	
	pcl::PCLPointCloud2 cloud;
	sensor_msgs::PointCloud2 output;

	pcl::io::loadPCDFile("/home/zjh/cloud_test/table_scene_lms400.pcd",cloud);
	pcl::fromPCLPointCloud2 (cloud,*cloud2);
    std::cout << typeid(cloud).name() << "     "<<   typeid(*cloud2).name() <<std::endl;
   
	// 创建一个长宽高分别是3cm的体素过滤器，cloud作为输入数据，cloud_filtered作为输出数据
	float leftSize = 0.03f;

	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud (cloud2);
	sor.setLeafSize (leftSize, leftSize, leftSize);
	sor.filter(*cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
	std::cout << cloud2->width << "     "<< cloud2->height  << "   " << cloud_filtered->width <<" " << cloud_filtered->height <<std::endl;

	pcl::toROSMsg(*cloud_filtered, output); 
    output.header.frame_id = "odom"; 
    ros::Rate loop_rate(1); 

    while (ros::ok()) { 
		pcl_pub.publish(output);
		ros::spinOnce(); 
		loop_rate.sleep(); 
    } 
    return 0;
}

看一下输出：
PointCloud after filtering: 5544 data points (x y z).460400 1 5544 1
由之前的460400 个点降到了 5544个点
为了效果明显参数设置的很了点，看一下降采的效果

点的密度明显有了变化。