devbox
2023面试高手
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS、PCL点云滤波模块整理_ros点云直通滤波

作者：2023面试高手 | 2024-02-21 09:45:27

赞

踩

ros点云直通滤波
  1. 点云滤波简介
    （1）哪些情况需要滤波？
    点云数据密度不规则需要平滑
    遮挡等问题造成离群点（outliers）需要去除
    大量数据需要进行下采样（ downsample ）
    噪声数据需要去除（noise remove）
    点云滤波通常为点云预处理的第一步，只有将噪声点、离群点、孔洞、数据压缩等做相关处理后，才能更好地进行特征提取、配准、曲面重建、可视化等应用。
    （2）PCL 中的 filters 模块
    https://link.zhihu.com/?target=http%3A//docs.pointclouds.org/trunk/group__filters.html
    Filters 模块包含32个类和5个函数，其依赖于 pcl::common, pcl::sample_consencus, pcl::search, pcl::kdtree, pcl::octree模块。
    下图是Filter继承关系：
    在这里插入图片描述
    class pcl::ApproximateVoxelGrid< PointT >
    class pcl::BilateralFilter< PointT >
    class pcl::BoxClipper3D< PointT >
    class pcl::Clipper3D< PointT >
    class pcl::ConditionalRemoval< PointT >
    class pcl::filters::Convolution< PointIn, PointOut >
    class pcl::filters::ConvolvingKernel< PointInT, PointOutT >
    class pcl::filters::GaussianKernel< PointInT, PointOutT >
    class pcl::filters::GaussianKernelRGB< PointInT, PointOutT >
    class pcl::CropBox< PointT >
    class pcl::CropBox< pcl::PCLPointCloud2 >
    class pcl::CropHull< PointT >
    class pcl::ExtractIndices< PointT >
    class pcl::ExtractIndices< pcl::PCLPointCloud2 >
    class pcl::Filter< PointT >
    class pcl::Filter< pcl::PCLPointCloud2 >
    class pcl::FilterIndices< PointT >
    class pcl::FilterIndices< pcl::PCLPointCloud2 >
    class pcl::FrustumCulling< PointT >
    class pcl::GridMinimum< PointT >
    class pcl::LocalMaximum< PointT >
    class pcl::MedianFilter< PointT >
    class pcl::NormalRefinement< NormalT >
    class pcl::NormalSpaceSampling< PointT, NormalT >
    class pcl::PlaneClipper3D< PointT >
    class pcl::ProjectInliers< PointT >
    class pcl::ProjectInliers< pcl::PCLPointCloud2 >
    class pcl::RandomSample< PointT >
    class pcl::RandomSample< pcl::PCLPointCloud2 >
    class pcl::SamplingSurfaceNormal< PointT >
    class pcl::ShadowPoints< PointT, NormalT >
    class pcl::VoxelGridOcclusionEstimation< PointT >

1 PassThrough 直通滤波
官方教程：Filtering a PointCloud using a PassThrough filter
1.class pcl::PassThrough< PointT >
2.class pcl::PassThrough< pcl::PCLPointCloud2 >

  pcl::PassThrough<pcl::PointXYZ> pass;    // 创建直通滤波器对象
  pass.setInputCloud (cloud);              // 输入点云
  pass.setFilterFieldName ("z");           // 设置过滤字段
  pass.setFilterLimits (0.0, 1.0);         // 设置范围
  //pass.setFilterLimitsNegative (true);   // 设置字段范围内的是保留（false）还是过滤掉（true）
  pass.filter (*cloud_filtered);           // 执行滤波，并存储结果

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

2.VoxelGrid 点云下采样
官方教程：Downsampling a PointCloud using a VoxelGrid filter
1.class pcl::ApproximateVoxelGrid< PointT >
2.class pcl::VoxelGrid< PointT >
3.class pcl::VoxelGrid< pcl::PCLPointCloud2 >
VoxelGrid类根据 输入的点云数据创建一个三维体素栅格，然后将每个体素内所有的点都用该体素内的点集的重心（centroid）来近似；ApproximateVoxelGrid类与VoxelGrid类基本相同，唯一不同在于其利用每一个体素的中心（center）来近似该体素内的点，相比于 VoxelGrid，计算速度稍快，但也损失了原始点云局部形态的精细度。

// 创建对象
pcl::VoxelGrid<pcl::PointXYZ> voxel;
// 输入点云
voxel.setInputCloud (cloud);
// 设置体素大小
voxel.setLeafSize (0.01f, 0.01f, 0.01f);
// 输出点云
voxel.filter (*cloud_filtered);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

关键成员函数：

// 通过向量设置体素栅格 leaf_size
void setLeafSize (const Eigen::Vector4f &leaf_size) 
// 通过 lx, ly, lz 分别设置体素栅格在 XYZ 3个方向上的尺寸
void setLeafSize (float lx, float ly, float lz)
// 设置是否对全部字段进行下采样
// 若需要在全部字段（包括颜色、强度等）下采样则设置参数 downsample 为 True 
// 仅对 XYZ 字段下采样则设置为 False 
void setDownsampleAllData ( bool downsample)

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

在这里插入图片描述
3. 去除 outliers
（1）统计分析 StatisticalOutlierRemoval
官方教程：Removing outliers using a StatisticalOutlierRemoval filter
class pcl::StatisticalOutlierRemoval< PointT >
class pcl::StatisticalOutlierRemoval< pcl::PCLPointCloud2 >

对每个点的邻域进行统计分析，计算所有邻点的平均距离，假如得到的分布为高斯分布，就可以得到均值和标准差，平均距离在区间 [公式] + std_mul * [公式] 之外的点视为离群点，std_mul为表示标准差倍数的阈值。

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);               // 设置临近点个数
  sor.setStddevMulThresh (1.0);    // 设置阈值，判断是否为离群点
  sor.filter (*cloud_filtered);

  • 1
  • 2
  • 3
  • 4
  • 5

结果：右图中红色与绿色分别表示滤波前后 mean k-nearest neighbor distances，可见离群点（outliers）变少。
在这里插入图片描述
（2）半径范围 RadiusOutlierRemoval
官方教程：Removing outliers using a Conditional or RadiusOutlier removal
class pcl::RadiusOutlierRemoval< PointT >
class pcl::RadiusOutlierRemoval< pcl::PCLPointCloud2 >

通过指定空间半径范围内的邻点数量来判定是否为离群点，如下图所示，若设定搜索半径为 d，最少的邻点数量为 1，则黄点将被视为离群点，若最少的邻点数量为2，则黄点和绿点都被视为离群点。
在这里插入图片描述

pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(cloud);
outrem.setRadiusSearch(0.8);          // 设置搜索半径
outrem.setMinNeighborsInRadius (2);   // 设置最少的邻点数量
outrem.filter (*cloud_filtered);

  • 1
  • 2
  • 3
  • 4
  • 5

4. 通过点云索引提取点云子集
官方教程：Extracting indices from a PointCloud
class pcl::ExtractIndices< PointT >
class pcl::ExtractIndices< pcl::PCLPointCloud2 >

// 下采样
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);

// 参数化分割
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);

// 提取索引
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);       // 设置分割后的内点为需要提取的点集
extract.setNegative (false);        // 设置提取内点
extract.filter (*cloud_p);          // 提取并保存至 cloud_p

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26

在这里插入图片描述在这里插入图片描述在这里插入图片描述
ROS、PCL点云过滤模块整理
前言
点云滤波模块常见的模块包括：Pass Through直通滤波、VoxelGrid过滤、StatisticalOutlierRemoval过滤、点云投影、提取索引等；今天介绍前三种过滤模块：直通滤波、体素滤波、统计滤波。

直通滤波：快速过滤掉用户自定义区间范围内的点云
体素滤波：在分割、配准前，如果点云数量太多会影响后续时间。此时，需要对点云进行下采样处理，体素滤波为采用体素网格方法采样，减少点云数量。
统计滤波：统计滤波往往去除离群点，利用统计分析技术删除噪声异常值等。
下面对原始点云分别进行三种滤波处理；

一、直通滤波

  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);           //执行滤波

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
/*********************************************
*滤波器：直通滤波器
*  功能 ：快速去除不在规定区间范围内的点； 
*  时间 ：2019.5.19
*  明天5.20，还在整理代码，单身狗的悲哀；
*********************************************/
#include <iostream>
#include <pcl/filters/passthrough.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>

using namespace std;

int main(int argc, char** argv)
{
	//读取点云
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_fz(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_fy(new pcl::PointCloud<pcl::PointXYZRGB>);
	reader.read("E:/code/date/coal_final.pcd", *cloud);

	//直通滤波
	pcl::PassThrough<pcl::PointXYZRGB> pass_z;//设置滤波器对象
	//参数设置
	pass_z.setInputCloud(cloud);
	pass_z.setFilterFieldName("z");
	//z轴区间设置
	pass_z.setFilterLimits(2.3,6);
	//设置为保留还是去除
	pass_z.setFilterLimitsNegative(true);
	pass_z.filter(*cloud_fz);

	//直通滤波
	pcl::PassThrough<pcl::PointXYZRGB> pass_y;//设置滤波器对象

	//参数设置
	pass_y.setInputCloud(cloud_fz);
	pass_y.setFilterFieldName("y");
	//z轴区间设置
	pass_y.setFilterLimits(-1.5, 0.9);
	//pass_y.setFilterLimitsNegative(false);
	pass_y.filter(*cloud_fy);

	pcl::PCDWriter writer;
	writer.write("E:/code/date/coal_520.pcd",*cloud_fy);

	//可视化
	pcl::visualization::PCLVisualizer viewer("cloud_viewer");
	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(cloud_fy);
	//添加坐标轴
	//viewer.addCoordinateSystem();
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return(0);

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63

在这里插入图片描述
可以看出，经过直通滤波后，大量杂乱的点云被直接去除。下面，对处理好的点云进行一次统计滤波。
二、统计滤波

#include <iostream>
#include <pcl/filters/statistical_outlier_removal.h>//统计滤波 头文件
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>

using namespace std;

int main(int argc, char** argv)
{
	//读取点云
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>);
	

	reader.read("E:/code/date/coal_520.pcd", *cloud);

	//统计滤波
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> outrem;//创建统计滤波对象

	//参数设置
	outrem.setInputCloud(cloud);
	outrem.setMeanK(200);//附近邻近点数
	outrem.setStddevMulThresh(1);//判断是否离群点
	outrem.filter(*cloud_f);
	
	//储存
	pcl::PCDWriter writer;
	writer.write("E:/code/cpp/filter/coal_.pcd", *cloud);

	//可视化
	pcl::visualization::PCLVisualizer viewer("cloud_viewer");

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(cloud_f);
	//viewer.addCoordinateSystem();
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return(0);

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46

在这里插入图片描述
经过统计滤波后，更多离群点或噪声被过滤。下面对点云进行下采样。
三、体素滤波

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;  //创建体素滤波器
  sor.setInputCloud (cloud);                //设置点云输入
  sor.setLeafSize (0.01f, 0.01f, 0.01f);    //设置滤波的体素大小，0.01m立方体
  sor.filter (*cloud_filtered);             //执行滤波

  • 1
  • 2
  • 3
  • 4
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace pcl;

int main(int argc, char** argv)
{

	//读取点云
	pcl::PCDReader reader;
	PointCloud<pcl::PointXYZRGB>::Ptr cloud(new PointCloud<pcl::PointXYZRGB>);
	PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new PointCloud<pcl::PointXYZRGB>);
	reader.read("E:/code/cpp/filter/coal_.pcd",*cloud);
	cout << "before_filter :"<<cloud->points.size()<<endl;

	//体素滤波
	pcl::VoxelGrid<pcl::PointXYZRGB> vox;
	vox.setInputCloud(cloud);
	vox.setLeafSize(0.01f,0.01f,0.01f);//体素网格大小
	vox.filter(*cloud_f);
	cout << "after filter :" << cloud_f->points.size()<<endl;


	//点云可视化
	pcl::visualization::PCLVisualizer viewer("cloud_viewer");
	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(cloud_f);
	viewer.addCoordinateSystem();

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return(0);
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41

在这里插入图片描述
在这里插入图片描述

可见，体素滤波下采样后，点云结构不变，故配准、分割前为了减少时间，往往对点云进行下采样处理。

一、点云投影

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace pcl;

int main(int argc, char** argv)
{
	//读取文件
	PCDReader reader;
	PCDWriter writer;
	PointCloud<pcl::PointXYZRGB>::Ptr cloud(new PointCloud<pcl::PointXYZRGB>);
	PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new PointCloud<pcl::PointXYZRGB>);
	reader.read("E:/code/date/Coal.pcd", *cloud);

	//我们使用平面模型，其中ax + by + cz + d = 0，其中a = b = d = 0，并且c = 1，或者换句话说，XY平面。
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);
	coefficients->values[0] = coefficients->values[1] = 0;
	coefficients->values[2] = 1.0;
	coefficients->values[3] = 0;

	//我们创建投影对象，并使用上面定义的模型作为投影模型
	pcl::ProjectInliers<pcl::PointXYZRGB> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_f);

	//可视化
	pcl::visualization::PCLVisualizer viewer("cloud_viewer");

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(cloud_f);
	//viewer.addCoordinateSystem();
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	return(0);
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46

原始点云:
在这里插入图片描述
投影后的点云:
在这里插入图片描述
【补充】
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

  • 1
  • 2

打开package.xml，在文本后添加

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>

  • 1
  • 2

示例程序（新手上路）
这是原始的示例程序：

#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 ();
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38

在CMakeLists.txt下添加新创建的包（每个新创建的包都要在CMakeLists.txt下添加路径）：

add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})

  • 1
  • 2

这个示例程序并没有进行任何滤波。
接下来是三种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 ();
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56

体素滤波

#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 ();
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55

半径滤波

#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 ();
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58

最后在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
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

注：（1）根据自己的滤波顺序，调整在滤波中的输入和输出。
（2）如果是在rviz中显示滤波图像，建议不要把体素滤波放在最后。
（3）个人测试：octomap支持的体素滤波的最小为0.001。
（4）体素滤波值太小而半径滤波的值太大会导致无法显示（全部过滤掉了）。
现在已经把三种滤波的.cpp写完了，接下来就是如何在ROS中调用。
3、将滤波加入ROS
这里主要实现滤波后的图像在rviz下显示。
修改octomap
参考：https://blog.csdn.net/zhang970187013/article/details/81098175
第三部分对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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

在sensor_manager.launch.xml文件中修改参数控制显示的精度：

 <param name="octomap_resolution" type="double" value="0.05" />

  • 1

修改启动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"/>

  • 1
  • 2
  • 3
  • 4
  • 5

启动node和remap的格式要求参考：http://wiki.ros.org/roslaunch/XML/node和http://wiki.ros.org/roslaunch/XML/remap

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/122677
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号