PCL 滤波去除噪点StatisticalOutlierRemoval

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

滤波前：

滤波后的效果：