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标准库头文件
#include //C++标准输入输出库
#include 
/*
  以下是pointcloud2转化的必要的头文件
*/
#include 
#include 
#include 
#include 
#include 

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 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("/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文件中添加：

libpcl-all-dev
  libpcl-all

然后，大功告成了，以下是转化结果：

原图：

转化后：