PCL 滤波去除噪点StatisticalOutlierRemoval
激光雷达扫描周围环境的时候,周围环境材料不同,反射不同,会产生不同密度的点云,同时,如果有测量偏差会导致更加稀疏的异常值出现,从而使点云生成结果质量更差。这样将会导致局部点云特征的估计更加复杂,很可能导致估计出错误值。
离群点滤波是一种常见的去除噪点的方法。它通过计算一个点的邻域内的点和该点的距离,来判断该点是否为噪点。如果该点距离邻域内的点的平均距离超过一定的阈值,则认为该点为噪点。
StatisticalOutlierRemoval 是一种基于统计学的离群点滤波方法。它通过计算每个点与其邻域内点的距离的标准差和平均值,来判断该点是否为噪点。如果该点与邻域内点的距离超过一定倍数的标准差,则认为该点为噪点。
实现代码如下:
#include <ros/ros.h>
#include <iostream>
#include "pcl_ros/point_cloud.h"
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "nav_msgs/OccupancyGrid.h"
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/transforms.h>
#include <visualization_msgs/Marker.h>
#include <string>
int main (int argc, char** argv)
{
//初始化节点
ros::init(argc, argv, "serial_example_node");
//声明节点句柄
ros::NodeHandle nh;
std::string file_path;
nh.param("file_path", file_path, file_path);
ros::Publisher cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/marker/bottom", 1);
std::string pcd_path = file_path + "map.pcd";
//加载点云:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_path.c_str(), *cloud) == -1)
{
std::cout << "Failed to load PCD file.";
return -1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr filterd_cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::cout<<"cloud.points.szie() = "<< cloud->points.size()<<std::endl;
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (cloud); //
sor.setMeanK (50);
sor.setStddevMulThresh (0.1);
sor.filter (*filterd_cloud);
//指定循环的频率
ros::Rate loop_rate(5);
while(ros::ok())
{
filterd_cloud->header.frame_id = "map";
filterd_cloud->header.stamp = ros::Time::now().toSec();
cloud_pub.publish(filterd_cloud);
//处理ROS的信息,比如订阅消息,并调用回调函数
ros::spinOnce();
loop_rate.sleep();
}
}
滤波前:
滤波后的效果: