使用kinect2获取点云数据，滤波保存转为octomap

准备

硬件：Kinect v2

系统：Ubuntu 16.04    ROS kinetic

前期准备：安装Kinect相关驱动，参考博客：安装kinect驱动配置

在Rviz中查看Kinect采集到的点云信息

驱动安装完成以后，可以连接上设备，使用以下命令测试一下能否在Rviz中看到点云信息：

roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true

rosrun rviz rviz

当Rviz打开以后，将Fixed Frame更改为kinect2_link,点击Add按钮添加PointCloud2,展开PointCloud2后，将Topic改为/kinect2/sd/points就可以看到点云信息。

点云PCL滤波

参考博客：ROS、PCL点云滤波

创建ROS package

cd ~/catkin_ws/src
catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs

打开package.xml，在文本后添加

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>

 直通滤波

保留或删除某一轴线特定范围内的点，改变视野范围。

对于Kinect v2来说，其相机坐标系为：红外镜头所在位置为原点，相机照射的正前方为Z的正方向，相机照射方向的左侧为X正方向，相机照射方向的上方为Y正方向。

先过滤Z方向：

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//添加引用
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
 
ros::Publisher pub;
 
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_passed;
 
  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);
 
  // Perform the actual filtering
  pcl::PassThrough<pcl::PCLPointCloud2> pass;  //定义直通滤波对象
    // build the filter
  pass.setInputCloud (cloudPtr);
  pass.setFilterFieldName ("z