ROS、PCL点云滤波
发布时间
阅读量：
阅读量
硬件：Kinect2、UR5。
软件：Ubuntu16.04、ROS kinetic。
未经处理的Kinect点云包含噪声、视野范围太大，所以需要对其进行处理。我采用了直通、半径、体素三种方式。
以下内容纯属个人学习，如有错误，欢迎指正，不胜感激！！
1、PCL滤波学习
官方教程：http://pointclouds.org/documentation/tutorials/index.php#filtering-tutorial
参考教程：<>
<>
<>
下面是我所使用的三种滤波，仅有滤波器的主体和参数设置部分（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
参考我的另一篇博文：<>
第三部分对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/node和http://wiki.ros.org/roslaunch/XML/remap
全部评论 (0)
还没有任何评论哟~