PCL | 直通滤波

最新推荐文章于 2024-09-21 21:34:53 发布

TwilightZrui

最新推荐文章于 2024-09-21 21:34:53 发布

阅读量376

点赞数

文章标签：自动驾驶

本文链接：https://blog.csdn.net/Twilightzr/article/details/120303183

版权

该博客主要展示了如何使用PCL库对ROS中的sensor_msgs/PointCloud2消息进行滤波处理。通过实例化滤波器，如VoxelGrid和Passthrough，对点云数据进行降噪和裁剪，实现点云的预处理。代码中首先将ROS点云转换为PCL格式，然后应用滤波器，最后将滤波后的点云转换回ROS格式并发布。

摘要由CSDN通过智能技术生成

/***********************************************************
关于使用sensor_msgs/PointCloud2，
***********************************************************/

#include <ros/ros.h>
#include<iostream>
// PCL 的相关的头文件
#include <pcl/io/pcd_io.h>
#include <sensor_msgs/PointCloud2.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>
using namespace std;
//申明发布器
ros::Publisher pub;
 //回调函数
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)  //特别注意的是这里面形参的数据格式
{
/*
-----------------------------------------------
 // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;    //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); 
  pcl::PCLPointCloud2 cloud_filtered;     //存储滤波后的数据格式

  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  // 进行一个滤波处理
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;   //实例化滤波
  sor.setInputCloud (cloudPtr);     //设置输入的滤波
  sor.setLeafSize (0.9f, 0.9f, 0.9f);   //设置体素网格的大小
  sor.filter (cloud_filtered);      //存储滤波后的点云

  // 再将滤波后的点云的数据格式转换为ROS 下的数据格式发布出去
  sensor_msgs::PointCloud2 output;   //声明的输出的点云的格式
  pcl_conversions::fromPCL(cloud_filtered, output);    //第一个参数是输入，后面的是输出

  //发布命令
  pub.publish (output);
-----------------------------------------------
  */

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

	//2.取得点云坐标极值
	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;

  // 原点云获取后进行滤波
  cout << "->filtering..." << endl;
  pcl::PassThrough<pcl::PointXYZ> pass;// 创建滤波器对象 直通滤波
  pass.setInputCloud (pcl_cloud_msg);//设置输入点云
  pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
  pass.setFilterLimits (-1.65, 0.5);//可接受的范围为（0.0，1.0） 
  //pass.setFilterLimitsNegative (true);//设置保留范围内 还是 过滤掉范围内
  pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
  pass.filter (*cloud_filtered); //执行滤波，保存过滤结果在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 ();
}