ros开发中用PCL Voxel_grid处理PointCloud2点云格式数据

在用双目摄像头进行视觉测距、识别时,会经常用到点云格式,PointCloud2格式的数据.我们将PointCloud2转化成costmap_2d,生成珊格地图。然后进行路径规划等操作。在实际测试时，由于PointCloud2的点云格式太大，懂则几十万个点，系统在运行转化时负荷太大，又由于性能影响，可能会有生成costmap珊格地图卡滞等情况。甚至无法达到设定的刷新频率。

这种情况下，可以使用pcl的Voxel_grid体素珊格滤波器对PointCloud2进行下采样，大大减少点云点的数量，提高效率和转化速率，减小系统负担。

由于我们是在ROS平台进行开发和调试，通过ROS中的TOPIC接受到来自摄像头的points数据，然后对数据进行下采样滤波后输出。

在ROS平台下使用PCL，官方文档给出了比较好的例子，参见http://wiki.ros.org/pcl/Tutorials#pcl.2BAC8-Tutorials.2BAC8-hydro.Add_the_source_file_to_CMakeLists.txt

首先，以下是源代码，头文件是必须要添加的。

#include<ros/ros.h> //ros标准库头文件
#include<iostream> //C++标准输入输出库
#include <stdio.h>
/*
  以下是pointcloud2转化的必要的头文件
*/
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>

static const std::string pointstopic = "/camera/depth/points";//设置topic名称

ros::Publisher points_pub;//定义ROS消息发布器
/*以下为滤波函数，首先需要将sensor_msg::PointCloud2格式转化成pcl::PCLPointCloud2格式，然后再使用VoxelGrid滤波 */
void pointsdo(const sensor_msgs::PointCloud2ConstPtr& msg)
{

pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 msg_filtered;
pcl_conversions::toPCL(*msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); //设置需要过滤的点云给滤波对象
sor.setLeafSize(0.02,0.02,0.02); //设置滤波时创建的体素大小为2cm立方体，通过设置该值可以直接改变滤波结果，值越大结果越稀疏
sor.filter(msg_filtered); //执行滤波处理，存储输出msg_filtered
sensor_msgs::PointCloud2 point_output;
pcl_conversions::fromPCL(msg_filtered, point_output);
points_pub.publish(point_output);
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "realcam_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe (pointstopic, 1, pointsdo);
points_pub=nh.advertise<sensor_msgs::PointCloud2>("/camera/depth/points_filter", 1);
ros::spin();
return 0;
}

在ROS下，还需要更改Cmakelist.txt和package.xml文件，在Cmakelist.txt中添加以下语句：

find_package(catkin REQUIRED COMPONENTS
cmake_modules
geometry_msgs
message_generation
roscpp
sensor_msgs
std_msgs
nodelet
pcl_ros
)
find_package(PCL 1.6 REQUIRED)

在package.xml文件中添加：
<build_depend>libpcl-all-dev</build_depend>
<run_depend>libpcl-all</run_depend>
然后，大功告成了，以下是转化结果：

原图：



转化后：