以下是对 PCL(Point Cloud Library)中各种滤波方法的总结,包括原理、所需头文件、机器人 SLAM 中的代码应用实例及使用说明:
一、体素网格滤波(Voxel Grid Filter)
-
原理:
- 体素网格滤波将三维空间划分为一个个小的立方体,即体素(Voxel)。对于每个体素,根据用户设定的体素尺寸,通过某种策略(如取平均值或中心点)将体素内的多个点合并为一个点,以此减少点云数据量。这样既可以保持点云的大致形状,又能显著降低点的数量,提高后续处理的效率。
-
头文件:
pcl/filters/voxel_grid.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "voxel_grid_filter_slam");
ros::NodeHandle nh;
// 订阅原始点云话题
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
// 创建体素网格滤波器
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(cloud);
// 设置体素的尺寸,可根据实际情况调整
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 执行滤波操作
voxel_filter.filter(*filtered_cloud);
// 将过滤后的点云转换为 ROS 消息
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
// 发布过滤后的点云
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 首先,通过
pcl::fromROSMsg
将sensor_msgs::PointCloud2
类型的输入点云消息转换为pcl::PointCloud<pcl::PointXYZ>
类型的点云。 - 创建
pcl::VoxelGrid<pcl::PointXYZ>
滤波器并设置输入点云。 - 使用
setLeafSize
设定体素的尺寸。 - 调用
filter
方法进行滤波,将结果存储在filtered_cloud
中。 - 最后通过
pcl::toROSMsg
将过滤后的点云转换为sensor_msgs::PointCloud2
并发布。
二、直通滤波(Pass Through Filter)
-
原理:
- 允许用户根据指定的维度(如
x
,y
,z
)和范围,仅保留该维度上处于指定范围内的点。例如,设置z
维度的范围为 [0.0, 1.0],则会过滤掉z
坐标不在此范围的点,可用于去除背景或提取特定区域内的点云。
- 允许用户根据指定的维度(如
-
头文件:
pcl/filters/passthrough.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "pass_through_filter_slam");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
pcl::PassThrough<pcl::PointXYZ> pass_filter;
pass_filter.setInputCloud(cloud);
// 选择过滤维度
pass_filter.setFilterFieldName("z");
// 设置过滤范围
pass_filter.setFilterLimits(0.0, 1.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pass_filter.filter(*filtered_cloud);
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 接收
sensor_msgs::PointCloud2
消息并转换为pcl::PointCloud<pcl::PointXYZ>
点云。 - 创建
pcl::PassThrough<pcl::PointXYZ>
滤波器,设置过滤维度和范围。 - 执行过滤操作,将结果转换为
sensor_msgs::PointCloud2
并发布。
三、统计离群点去除滤波(Statistical Outlier Removal Filter)
-
原理:
- 对每个点计算其邻域内的统计信息(如平均距离),若该点与其邻域点的平均距离超过一定的标准差倍数,将其视为离群点并去除。这有助于去除因传感器噪声或测量误差产生的离群点,提高点云质量。
-
头文件:
pcl/filters/statistical_outlier_removal.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "statistical_outlier_removal_filter_slam");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor_filter;
sor_filter.setInputCloud(cloud);
// 考虑每个点的邻域点数
sor_filter.setMeanK(50);
// 标准差倍数,超过此倍数的点视为离群点
sor_filter.setStddevMulThresh(1.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
sor_filter.filter(*filtered_cloud);
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 转换输入点云消息为 PCL 点云。
- 创建
pcl::StatisticalOutlierRemoval<pcl::PointXYZ>
滤波器,设置邻域点数和标准差倍数。 - 过滤离群点,将结果转换为 ROS 消息并发布。
四、半径离群点去除滤波(Radius Outlier Removal Filter)
-
原理:
- 对于每个点,在设定的半径范围内搜索其邻域点,若邻域内的点数少于指定阈值,将该点视为离群点并去除。适用于去除稀疏分布的离群点。
-
头文件:
pcl/filters/radius_outlier_removal.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "radius_outlier_removal_filter_slam");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
pcl::RadiusOutlierRemoval<pcl::PointXYZ> rad_filter;
rad_filter.setInputCloud(cloud);
// 搜索半径
rad_filter.setRadiusSearch(0.5);
// 邻域内最少点数
rad_filter.setMinNeighborsInRadius(2);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
rad_filter.filter(*filtered_cloud);
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 接收输入点云消息并转换为 PCL 点云。
- 创建
pcl::RadiusOutlierRemoval<pcl::PointXYZ>
滤波器,设置搜索半径和邻域内最少点数。 - 执行过滤操作,将结果转换为 ROS 消息并发布。
五、条件滤波(Conditional Filter)
-
原理:
- 允许用户自定义复杂的过滤条件,基于点云的属性(如坐标、颜色、法线等),使用逻辑运算符(如 AND、OR)组合多个条件,实现灵活的过滤操作。
-
头文件:
pcl/filters/conditional_removal.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/conditional_removal.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "conditional_filter_slam");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
// 创建条件对象
pcl::ConditionAnd<pcl::PointXYZ>::Ptr condition(new pcl::ConditionAnd<pcl::PointXYZ>());
// 仅保留 x 坐标在 0.0 到 1.0 之间的点
condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, 0.0)));
condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 1.0)));
pcl::ConditionalRemoval<pcl::PointXYZ> cond_filter;
cond_filter.setInputCloud(cloud);
cond_filter.setCondition(condition);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
cond_filter.filter(*filtered_cloud);
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 转换输入点云消息为 PCL 点云。
- 使用
pcl::ConditionAnd<pcl::PointXYZ>
创建条件对象,添加条件(如x
坐标范围)。 - 创建
pcl::ConditionalRemoval<pcl::PointXYZ>
滤波器,设置输入点云和条件。 - 执行过滤操作,将结果转换为 ROS 消息并发布。
六、双通滤波(Double Pass Filter)
-
原理:
- 类似于直通滤波,但可以在两个不同维度上进行连续的范围过滤,以截取更复杂的空间区域。例如,先在
z
轴上过滤,再在x
轴上过滤,实现对三维空间中长方体区域的截取。
- 类似于直通滤波,但可以在两个不同维度上进行连续的范围过滤,以截取更复杂的空间区域。例如,先在
-
头文件:
pcl/filters/passthrough.h
-
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "double_pass_filter_slam");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
pcl::PassThrough<pcl::PointXYZ> pass_filter_z, pass_filter_x;
pass_filter_z.setInputCloud(cloud);
pass_filter_z.setFilterFieldName("z");
pass_filter_z.setFilterLimits(0.0, 1.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr intermediate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pass_filter_z.filter(*intermediate_cloud);
pass_filter_x.setInputCloud(intermediate_cloud);
pass_filter_x.setFilterFieldName("x");
pass_filter_x.setFilterLimits(0.5, 2.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pass_filter_x.filter(*filtered_cloud);
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
使用说明:
- 首先将输入点云转换为 PCL 点云。
- 使用
pcl::PassThrough<pcl::PointXYZ>
进行第一次维度(如z
轴)的过滤。 - 对中间结果再进行另一个维度(如
x
轴)的过滤。 - 将最终结果转换为 ROS 消息并发布。
七、卷积滤波(Convolution Filter)
原理:
卷积滤波是一种基于卷积操作的点云滤波方法,它将传统的二维图像卷积操作扩展到三维点云数据上。其基本原理是通过定义一个卷积核(Kernel),该卷积核是一个三维的权重矩阵,在点云的每个点周围的邻域内进行滑动,并根据卷积核中的权重对邻域内的点的属性(如强度、颜色等)进行加权平均计算,从而更新点的属性值。通过这种方式,可以实现点云数据的平滑处理,去除噪声,或者提取点云的特征信息。
头文件:pcl/filters/convolution_3d.h
代码示例(机器人 SLAM 应用):
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/convolution_3d.h>
#include <pcl/common/common_headers.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "convolution_filter_slam");
ros::NodeHandle nh;
// 订阅原始点云话题
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("input_cloud_topic", 1, [&](const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg) {
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*input_cloud_msg, *cloud);
// 创建卷积滤波器
pcl::filters::Convolution3D<pcl::PointXYZI, pcl::PointXYZI> convolution_filter;
convolution_filter.setInputCloud(cloud);
// 创建卷积核,这里使用简单的平均核作为示例
pcl::filters::Convolution3D<pcl::PointXYZI, pcl::PointXYZI>::Kernel::Ptr kernel(new pcl::filters::Convolution3D<pcl::PointXYZI, pcl::PointXYZI>::Kernel);
// 定义一个简单的平均核,大小为 3x3x3
float kernel_matrix[3][3][3] = {
{{1.0 / 27.0, 1.0 / 27.0, 1.0 / 27.0},
{1.0 / 27.0, 1.0 / 27.0, 1.0 / 27.0},
{1.0 / 27.0, 1.0 / 27.0, 1.0 / 27.0}};
// 设置卷积核矩阵
kernel->setKernelMatrix(kernel_matrix);
// 设置卷积核的大小
kernel->setKernelSize(3, 3, 3);
// 将卷积核设置到滤波器中
convolution_filter.setKernel(*kernel);
// 存储过滤后的点云
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZI>);
// 执行卷积滤波操作
convolution_filter.process(*filtered_cloud);
// 将过滤后的点云转换为 ROS 消息
sensor_msgs::PointCloud2 output_cloud_msg;
pcl::toROSMsg(*filtered_cloud, output_cloud_msg);
// 发布过滤后的点云
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("filtered_cloud_topic", 1);
pub.publish(output_cloud_msg);
});
ros::spin();
return 0;
}
代码解释:
-
订阅和转换点云:
- 首先,通过
ros::Subscriber
订阅名为"input_cloud_topic"
的sensor_msgs::PointCloud2
类型的原始点云消息。 - 使用
pcl::fromROSMsg
将sensor_msgs::PointCloud2
消息转换为pcl::PointCloud<pcl::PointXYZI>
类型的点云,这里使用pcl::PointXYZI
类型是因为我们可能需要处理具有强度信息的点云。
- 首先,通过
-
创建卷积滤波器:
- 创建
pcl::filters::Convolution3D<pcl::PointXYZI, pcl::PointXYZI>
类型的卷积滤波器对象convolution_filter
,并将输入点云设置为刚刚转换得到的点云。
- 创建
-
创建和设置卷积核:
- 创建
pcl::filters::Convolution3D<pcl::PointXYZI, pcl::PointXYZI>::Kernel
类型的卷积核对象kernel
。 - 定义一个简单的 3x3x3 平均卷积核矩阵,每个元素的值为
1.0 / 27.0
,以实现平均卷积。 - 使用
kernel->setKernelMatrix
方法将卷积核矩阵设置到卷积核对象中。 - 使用
kernel->setKernelSize(3, 3, 3)
设置卷积核的大小。
- 创建
-
设置滤波器的卷积核:
- 使用
convolution_filter.setKernel(*kernel)
将创建好的卷积核设置到卷积滤波器中。
- 使用
-
执行滤波操作:
- 创建
pcl::PointCloud<pcl::PointXYZI>::Ptr
类型的filtered_cloud
存储过滤后的点云。 - 调用
convolution_filter.process(*filtered_cloud)
执行卷积滤波操作,将结果存储在filtered_cloud
中。
- 创建
-
转换和发布结果:
- 使用
pcl::toROSMsg
将过滤后的点云转换为sensor_msgs::PointCloud2
类型的消息。 - 通过
ros::Publisher
发布过滤后的点云消息到"filtered_cloud_topic"
。
- 使用
使用说明:
- 确保你已经安装并配置好 PCL 库和 ROS 环境,并且在
CMakeLists.txt
文件中正确添加了所需的库依赖。 - 运行该节点时,它会订阅
"input_cloud_topic"
上的点云消息,对其进行卷积滤波处理,并将结果发布到"filtered_cloud_topic"
上。 - 你可以根据需要调整卷积核的大小和内容,以实现不同的滤波效果,例如使用不同的核矩阵进行平滑、锐化或特征提取等操作。
请根据实际需求修改代码中的参数,例如卷积核的大小和元素值,以达到不同的点云滤波和特征提取效果。同时,可以根据点云的具体属性选择不同的点类型,如 pcl::PointXYZ
或 pcl::PointXYZRGB
等。
此外,在实际应用中,还可以使用更复杂的卷积核和不同的卷积模式(如不同的边界处理方式),以满足更复杂的点云处理需求。如果你需要对其他属性进行卷积,只需调整点云类型和相应的卷积核矩阵即可。
希望这个示例对你在机器人 SLAM 中的点云滤波工作有所帮助,让你更好地理解和使用卷积滤波。如果你在使用过程中遇到任何问题,可以进一步调整代码或向我咨询。
标签:include,PCL,ros,filter,pcl,点云,2.0,cloud From: https://blog.csdn.net/qq_15258067/article/details/145132491