使用PCL了组合滤波算法对ROSbag原始点云数据进行滤波

/***********************************************************
对ROSbag原始点云数据进行滤波
1.接收到ROS sensor_msgs/PointCloud2 的/lidar_center/velodyne_points数据，转化为PCL可以处理的pcl::PointCloud<pcl::PointXYZ>::Ptr
2.直通滤波：z轴范围为(-1.65, 0.5)
3.直通滤波：y轴范围为(-50, 50)
4.去除 outliers：统计学离群点移除过滤器移除噪点
5.将PCL点云转化为ROS点云
6.发布/output数据
***********************************************************/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include<iostream>
// PCL 的相关的头文件
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>  
//滤波的头文件
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波 头文件
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
using namespace std;

ros::Publisher pub; //申明发布器

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)  
{

	// PCL 第一代点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_msg(new pcl::PointCloud<pcl::PointXYZ>); //pcl_cloud_msg要转化成的点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_msg_fz(new pcl::PointCloud<pcl::PointXYZ>); 
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_msg_fy(new pcl::PointCloud<pcl::PointXYZ>); 
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_msg_fx(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
	
	// 转化：ROS 点云 -> PCL 第一代点云
  cout << "->transforming cloudpoint from ROS to pcl..." << endl;
	pcl::fromROSMsg(*input, *pcl_cloud_msg);

	//取得点云坐标极值
	pcl::PointXYZ minPt, maxPt;
	pcl::getMinMax3D(*pcl_cloud_msg, minPt, maxPt);
  //cout << "minPt.x : " << minPt.x << " maxPt.x : " << maxPt.y << endl;
  //cout << "minPt.y : " << minPt.x << " maxPt.y : " << maxPt.y << endl;
  //cout << "minPt.z : " << minPt.x << " maxPt.z : " << maxPt.y << endl;

  // Z轴方向直通滤波
  cout << "->filtering z..." << endl;
  pcl::PassThrough<pcl::PointXYZ> pass_z;
  pass_z.setInputCloud (pcl_cloud_msg);
  pass_z.setFilterFieldName("z");
  pass_z.setFilterLimits(-1.65, 0.5);
  pass_z.setFilterLimitsNegative(false);
  pass_z.filter(*pcl_cloud_msg_fz);

  // Y轴方向直通滤波
  cout << "->filtering y..." << endl;
  pcl::PassThrough<pcl::PointXYZ> pass_y;// 创建滤波器对象 直通滤波
  pass_y.setInputCloud (pcl_cloud_msg_fz);//设置输入点云
  pass_y.setFilterFieldName ("y");//滤波字段名被设置为Y轴方向
  pass_y.setFilterLimits (-50, 50);//可接受的范围为（0.0，1.0） 
  //pass.setFilterLimitsNegative (true);//设置保留范围内 还是 过滤掉范围内
  pass_y.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
  pass_y.filter (*pcl_cloud_msg_fy); //执行滤波，保存过滤结果在cloud_filtered

  //去除 outliers 统计分析 StatisticalOutlierRemoval 统计学离群点移除过滤器移除噪点
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (pcl_cloud_msg_fy);
  sor.setMeanK (50);               // 设置临近点个数
  sor.setStddevMulThresh (1.0);    // 设置阈值，判断是否为离群点
  sor.filter (*cloud_filtered);

  // 转化：PCL 第一代点云-> ROS 点云 
  cout << "->transforming point from PCL to ROS..." << endl;
  sensor_msgs::PointCloud2 output;  // ROS 点云
  pcl::toROSMsg(*cloud_filtered, output);  // PCL 第一代点云 -> ROS 点云

  //发布命令
  cout << "->publishing ROS pointcloud2..." << endl;
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // 初始化 ROS节点
  ros::init (argc, argv, "filter_test_node");
  ros::NodeHandle nh;   //声明节点的名称

  // 为接受点云数据创建一个订阅节点
  ros::Subscriber sub = nh.subscribe ("/lidar_center/velodyne_points", 1, cloud_cb);

  //创建ROS的发布节点
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // 回调
  ros::spin ();
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(cpoint)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  sensor_msgs
  std_msgs
  cv_bridge
  image_transport
  pcl_conversions
  pcl_ros
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)

################################################
## Declare ROS messages, services and actions ##
################################################

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

###################################
## catkin specific configuration ##
###################################
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test_pkg
#  CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

include_directories(
  ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
## add_executable(cpoint src/PointCloud2_to_pcl_node.cpp src/filter_test.cpp)

add_executable(filter_test_node src/filter_test.cpp)

target_link_libraries(filter_test_node ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})


target_link_libraries(filter_test_node ${catkin_LIBRARIES})

01-点云滤波Filtering *
Point Cloud Library
Removing outliers using a StatisticalOutlierRemoval filter
PCL点云滤波
https://zhuanlan.zhihu.com/p/66296677
【PCL笔记】Filtering 点云滤波
PCL：点云滤波汇总：算法原理 + 代码实现