使用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:点云滤波汇总:算法原理 + 代码实现
上一篇: 数据类型(ORACLE)