ROS、PCL点云滤波

硬件:Kinect2、UR5。

软件:Ubuntu16.04、ROS kinetic。

未经处理的Kinect点云包含噪声、视野范围太大,所以需要对其进行处理。我采用了直通、半径、体素三种方式。

以下内容纯属个人学习,如有错误,欢迎指正,不胜感激!!

1、PCL滤波学习

官方教程:http://pointclouds.org/documentation/tutorials/index.php#filtering-tutorial

参考教程:https://blog.csdn.net/fandq1223/article/details/53185053

                  https://blog.csdn.net/u013019296/article/details/70052319

                  https://blog.csdn.net/qq_34719188/article/details/79179430

下面是我所使用的三种滤波,仅有滤波器的主体和参数设置部分(ROS下只用到了这些)。

直通滤波

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

  pcl::PassThrough<pcl::PointXYZ> pass;    //创建滤波器
  pass.setInputCloud (cloud);              //设置点云输入
  pass.setFilterFieldName ("z");           //设置滤波的方向,z轴
  pass.setFilterLimits (0.0, 1.0);         //设置滤波的范围
  //pass.setFilterLimitsNegative (true);   //设置范围内点云保留还是过滤,默认保留
  pass.filter (*cloud_filtered);           //执行滤波

体素滤波

使用体素网格进行下采样,这么做减少了点云的数量和数据(并没有发现变化)、保留点云表面的形状体征,可以提高配准、表面重建等。

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  //创建体素滤波器
  sor.setInputCloud (cloud);                //设置点云输入
  sor.setLeafSize (0.01f, 0.01f, 0.01f);    //设置滤波的体素大小,0.01m立方体
  sor.filter (*cloud_filtered);             //执行滤波

半径滤波

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

    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;  //创建半径滤波
    outrem.setInputCloud(cloud);                      //设置点云输入
    outrem.setRadiusSearch(0.8);                      //设置查询半径,0.8m,并查询该邻域
    outrem.setMinNeighborsInRadius (2);               //该邻域内点的个数小于XX则删除
    outrem.filter (*cloud_filtered);                  //执行滤波

2、ROS下的PCL

官方教程:http://wiki.ros.org/pcl/Tutorials#pcl.2BAC8-Tutorials.2BAC8-hydro.Create_a_ROS_package

创建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>

示例程序(新手上路)

这是原始的示例程序,看不懂的出门右拐(开玩笑!具体的流程在示例程序之后会有说明)。

#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>

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;

  // Do data processing here...
  output = *input;

  // Publish the data.
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
  ros::spin ();
}

在CMakeLists.txt下添加新创建的包(每个新创建的包都要在CMakeLists.txt下添加路径):

add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})

这个示例程序并没有进行任何滤波。

接下来是三种ROS下的滤波。

框架可以参考ROS下的example_voxelgrid.cpp:

http://wiki.ros.org/pcl/Tutorials/hydro?action=AttachFile&do=view&target=example_voxelgrid.cpp

直通滤波

#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>

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::PassThrough<pcl::PCLPointCloud2> pass;
    // build the filter
  pass.setInputCloud (cloudPtr);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.3);
    // apply filter
  pass.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_pt;
  pcl_conversions::moveFromPCL(cloud_filtered, 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_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.005, 0.005, 0.005);
    // 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>


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::RadiusOutlierRemoval<pcl::PCLPointCloud2> outrem;
     // build the filter  
    outrem.setInputCloud(cloudPtr);
    outrem.setRadiusSearch(0.08);
    outrem.setMinNeighborsInRadius (60);
     // 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 ();
}

最后在CMakeLists.txt下添加三种滤波方式

add_executable(passthrough src/passthrough.cpp)
target_link_libraries(passthrough ${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})

注:(1)根据自己的滤波顺序,调整在滤波中的输入和输出。

       (2)如果是在rviz中显示滤波图像,建议不要把体素滤波放在最后。

       (3)个人测试:octomap支持的体素滤波的最小为0.001。

       (4)体素滤波值太小而半径滤波的值太大会导致无法显示(全部过滤掉了)。

现在已经把三种滤波的.cpp写完了,接下来就是如何在ROS中调用。

3、将滤波加入ROS

这里主要实现滤波后的图像在rviz下显示。

修改octomap

参考我的另一篇博文:https://blog.csdn.net/zhang970187013/article/details/81098175

第三部分对sensors_kinect.yaml修改,主要修改point_cloud_topic,实现octomap调用滤波后的输出(最后一个滤波的output)。

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /filtered_RadiusOutlierRemoval
    max_range: 2
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    filtered_cloud_topic: filtered_cloud

在sensor_manager.launch.xml文件中修改参数控制显示的精度:

 <param name="octomap_resolution" type="double" value="0.05" />

修改启动launch文件

之前一步虽然可以调用输出,但是并没有执行滤波处理,所以在启动文件上加入滤波处理。

这一部分也是参考前一步的博文,在launch中加入:

 <node pkg="my_pcl_tutorial" type="passthrough" name="Pass_Through" respawn="false">
  <remap from="/cloud_input" to="/kinect2/qhd/points" />
 </node>
 <node pkg="my_pcl_tutorial" type="radius_outlier_removal" name="Radius_OutlierRemoval" respawn="false"/>
 <node pkg="my_pcl_tutorial" type="voxel_grid" name="Voxel_Grid" respawn="false"/>

启动node和remap的格式要求参考:http://wiki.ros.org/roslaunch/XML/nodehttp://wiki.ros.org/roslaunch/XML/remap

 

 

 

  • 11
    点赞
  • 107
    收藏
    觉得还不错? 一键收藏
  • 9
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值