准备
硬件:Kinect v2

系统:Ubuntu 16.04    ROS kinetic

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

在Rviz中查看Kinect采集到的点云信息
驱动安装完成以后,可以连接上设备,使用以下命令测试一下能否在Rviz中看到点云信息:

roslaunch kinect2_bridge kinect2_bridge.launchpublish_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"); //将在相机照射方向z范围外的过滤
  pass.setFilterLimits (0.0, 1.3);
    // apply filter
  pass.filter (cloud_passed);
 
  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_pt;
  pcl_conversions::moveFromPCL(cloud_passed, cloud_pt);
 
  // Publish the data
  pub.publish (cloud_pt);
}
 
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "PassThrough");
  ros::NodeHandle nh;
 
  // Create a ROS subscriber for the input point cloud 输入
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/cloud_input", 1, cloud_cb);
 
  // Create a ROS publisher for the output point cloud 输出
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_PassThroughz", 1);
 
  // Spin
  ros::spin ();
}

同样的方法再过滤X方向:

#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 ("x"); //将在相机照射方向左右两侧x范围外的过滤
  pass.setFilterLimits (-0.4, 1.0);
    // apply filter
  pass.filter (cloud_passed);
 
  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_pt;
  pcl_conversions::moveFromPCL(cloud_passed, cloud_pt);
 
  // Publish the data
  pub.publish (cloud_pt);
}
 
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "PassThroughx");
  ros::NodeHandle nh;
 
  // Create a ROS subscriber for the input point cloud 输入
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/filtered_PassThroughz", 1, cloud_cb);
 
  // Create a ROS publisher for the output point cloud 输出
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_PassThrough", 1);
 
  // Spin
  ros::spin ();
}

体素滤波

使用体素网格进行下采样,这么做减少了点云的数量、保留点云表面的形状体征,可以提高配准、表面重建等。

#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/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_filtered;
 
  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);
 
  // Perform the actual filtering
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    // build the filter
    sor.setInputCloud (cloudPtr);   
    sor.setLeafSize (0.01, 0.01, 0.01); //数越小,点越多
    // apply filter
    sor.filter (cloud_filtered);
 
  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_vog;
  pcl_conversions::moveFromPCL(cloud_filtered, cloud_vog);
 
  // Publish the data
  pub.publish (cloud_vog);
}
 
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "VoxelGrid");
  ros::NodeHandle nh;
 
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/filtered_PassThrough", 1, cloud_cb);
 
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_VoxelGrid", 1);
 
  // Spin
  ros::spin ();
}

半径滤波

降噪,删除该图像内符合约束条件的点。

#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/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/io/pcd_io.h>
 
ros::Publisher pub;
int i=0;
 
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_filtered;
  pcl::PointCloud<pcl::PointXYZ>::Ptr savedata(new pcl::PointCloud<pcl::PointXYZ>);
 
  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);
 
  // Perform the actual filtering
    pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> outrem;
     // build the filter  
    outrem.setInputCloud(cloudPtr);
    outrem.setRadiusSearch(0.08); //半径
    outrem.setMinNeighborsInRadius (50);  //点数
     // apply filter
    outrem.filter (cloud_filtered);
 
  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_rad;
  pcl_conversions::moveFromPCL(cloud_filtered, cloud_rad); 
 
  // Publish the data
  pub.publish (cloud_rad);
}
 
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "RadiusOutlierRemoval");
  ros::NodeHandle nh;
 
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/filtered_VoxelGrid", 1, cloud_cb);
 
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_RadiusOutlierRemoval", 1);
 
  // Spin
  ros::spin ();
}

可以根据自己的滤波顺序,调整在滤波中的输入和输出。

将滤波加入ROS

以上的.cpp滤波程序写好保存在src文件夹下面。在CMakeLists.txt中添加:

add_executable(passthrough src/passthrough.cpp)
target_link_libraries(passthrough ${catkin_LIBRARIES})
 
add_executable(passthroughx src/passthroughx.cpp)
target_link_libraries(passthroughx ${catkin_LIBRARIES})
 
add_executable(radius_outlier_removal src/radius_outlier_removal.cpp)
target_link_libraries(radius_outlier_removal ${catkin_LIBRARIES})
 
add_executable(voxel_grid src/voxel_grid.cpp)
target_link_libraries(voxel_grid ${catkin_LIBRARIES})
 
add_executable(pcl_write src/pcl_write.cpp)
target_link_libraries(pcl_write ${catkin_LIBRARIES})

然后再catkin_make编译一下就可以生成可执行文件,然后如果这些文件一个个启动太麻烦,写了一个filter.launch文件:

<?xml version="1.0"?>
 
<launch>
<node pkg="my_pcl_tutorial" type="passthrough" name="passthrough">
	<remap from="cloud_input" to="kinect2/qhd/points" />
</node>
 <node pkg="my_pcl_tutorial" type="passthroughx" name="PassThroughx" respawn="false"/>
 <node pkg="my_pcl_tutorial" type="voxel_grid" name="Voxel_Grid" respawn="false"/>
 <node pkg="my_pcl_tutorial" type="radius_outlier_removal" name="Radius_OutlierRemoval" respawn="false"/>
 <node pkg="rviz" type="rviz" name="Rviz"/>
</launch>

运行

使用以下命令打开:

roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
roslaunch my_pcl_tutorial filter.launch

按照之前的步骤,只是将Topic改一下就可以。

保存点云数据

以下程序就可以将滤波后带颜色的点云数据保存:

#include<ros/ros.h>
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>
int i=0;
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
  i++;
  pcl::PointCloud<pcl::PointXYZRGBA> cloud;
  pcl::fromROSMsg(input, cloud);//从ROS类型消息转为PCL类型消息
  if(i==5){
	  pcl::io::savePCDFileASCII ("/home/lwl/install/octomap_tutor/data/pointcloud.pcd", cloud);//保存pcd,路径改为自己的
    ROS_INFO("Save the pointcloud seccessful!");
}
 
}
main (int argc, char **argv)
{
  ros::init (argc, argv, "pcl_write");
  ros::NodeHandle nh;
  ros::Subscriber bat_sub = nh.subscribe("filtered_RadiusOutlierRemoval", 10, cloudCB);//接收点云
  ros::spin();
  return 0;
}

在上面程序运行的基础上运行以下命令就可以进行保存:

rosrun my_pcl_tutorial pcl_write

打开保存的点云数据

cd 到保存点云数据的文件夹下面,使用以下命令查看:

pcl_viewer pointcloud.pcd

果提示没有安装查看点云数据的软件,安装提示安装以下即可:

转为octomap

参考博客:octomap的简介

如果需要octomap的话,可以进行下一步,它可以优雅地压缩、更新地图,并且分辨率可调!它以八叉树的形式存储地图,相比点云,能够省下大把的空间。

新建一个目录,拷贝octomap代码:

git clone https://github.com/OctoMap/octomap

如果下载失败可以直接按照链接在谷歌上下载,下载完成后,进入该目录,参照README.md进行安装:

mkdir build
cd build
cmake ..
make

如果编译没有给出任何警告,恭喜你编译成功。

转换pcd到octomap,按照参考博客操作即可。