欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-06-02 22:08:42
...
/***********************************************************
对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:点云滤波汇总:算法原理 + 代码实现

相关标签: 自动驾驶 算法