地图分很多种：稀疏的，稠密的，还有半稀疏的。稀疏的地图放大了看就是一个个离散的空间点，不过我们可以把它变成连续的稠密的网格，这个过程也叫点云的网格化。点云网格化需要对点云进行一系列处理

一般下面这几种情况需要进行点云滤波处理：

1. 点云数据密度不规则需要平滑

2. 因为遮挡等问题造成离群点需要去除

3. 大量数据需要降采样

4. 噪声数据需要去除

滤波程序

package.xml中要添加：

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

CMake按照第一篇里进行配置

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);

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

  pcl::PCLPointCloud2 cloud_filtered;   //存储滤波后的数据格式
  // 进行一个滤波处理
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor; //创建滤波对象
  sor.setInputCloud (cloudPtr);  //设置输入的滤波，将需要过滤的点云给滤波对象
  sor.setLeafSize (0.1, 0.1, 0.1);  //设置滤波时创建的体素大小为1cm立方体
  sor.filter (cloud_filtered);//执行滤波处理，存储输出cloud_filtered

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

int main (int argc, char** argv)
{
  ros::init (argc, argv, "filter_cloud");//声明节点的名称
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth/points", 1, cloud_cb);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("filtered_cloud", 1);

  ros::spin ();
}

点云合并

把相邻的点合并成一个点，一方面可以减少图像的数据量，另一方面可以增加点云的稳定度。个人感觉贴近滤波

直通滤波

多线雷达点云大部分的点都集中的中间区域，距离越远点云越稀疏，信息量也越小，应该筛选掉一些较远的点。这就用到了直通滤波，它是最为简单、粗暴的一种滤波方式，就是直接对点云的X、Y、Z轴的点云坐标约束来进行滤波。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

int main(int argc, char** argv)
{
    
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read("16line.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << cloud->points.size() << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered3(new pcl::PointCloud<pcl::PointXYZ>);

  // Create the filtering object 直通滤波
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("x");   // 设置操作的坐标轴
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered2);
  // filter range Y-axis
  // 注意多次滤波需要再次输入点云和调用filter函数
  pass.setInputCloud(cloud_filtered2);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(-5.0, 5.0);
  pass.filter(*cloud_filtered3);

  std::cerr << "Cloud after filtering: " << cloud_filtered3->points.size() << std::endl;

  // 读取pcd点云数据进行操作，最后保存为pcd文件
  pcl::PCDWriter writer;
  writer.write("16line_filtered.pcd", *cloud_filtered3, false);

  return 0;
}

pass.setFilterLimitsNegative(true);: 是否保存滤波的限制范围内的点云，默认为false，保存限制范围点云，true时候是相反。

条件滤波法

在某些情况下，我们也会需要去除给定区域内部的点，比如激光雷达扫描近处的点(可能就是装载雷达的车)，这些点通常对建图和感知不会有作用。

设定滤波条件进行滤波，点在范围内保留，不在范围内丢弃。

getFilterLimitsNegative 可以对选定范围取反，将其设为 true 时可以进行给定只保留给定范围内的点的功能。

体素滤波

实现降采样，既减少点云的数量，并同时保持点云的形状特征。创建一个三维体素，用体素内所有点的重心近似表示体素中的其他点，这样体素内所有点就用一个重心点最终表示。

使用小程序进行体素滤波，输入input.pcd，输出output.pcd

pcl_voxel_grid input.pcd output.pcd -leaf 0.03,0.03,0.03

半径滤波

对整个输入迭代一次，对于每个点进行半径R的邻域搜索(球体)，如果邻域点的个数低于某一阈值，则该点将被视为噪声点并被移除。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);      //待滤波点云
if (pcl::io::loadPCDFile("test.pcd", *cloud) < 0)
{
  PCL_ERROR("\a点云文件不存在！\n");
  system("pause");
  return -1;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //滤波后点云
pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror; //创建滤波器对象
ror.setInputCloud(cloud);           //设置待滤波点云
ror.setRadiusSearch(0.02);            //设置查询点的半径邻域范围
ror.setMinNeighborsInRadius(5);         //设置判断是否为离群点的阈值，即半径内至少包括的点数
//ror.setNegative(true);            //默认false，保存内点；true，保存滤掉的外点
ror.filter(*cloud_filtered);          //执行滤波，保存滤波结果于cloud_filtered

实现：

void radiusremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, double radius, int min_pts)
{
  pcl::KdTreeFLANN<pcl::PointXYZ> tree;
  tree.setInputCloud(cloud);
  vector<float> avg;
  for (int i = 0; i < cloud->points.size(); ++i)
  {
    vector<int> id(min_pts);
    vector<float> dist(min_pts);
    tree.nearestKSearch(cloud->points[i], min_pts, id, dist);
    // 是否是平方
    if (dist[dist.size() - 1] < radius)
    {
      cloud_filtered->push_back(cloud->points[i]);
    }
  }
}

构建一个 KD 树，对每个点进行范围搜索（最近邻搜索 nearestKSearch），最后判断邻近点数（最大距离）是否满足要求即可。

统计滤波

看过源码后得知， 计算均值 $\mu$ 时除以的是点云的点数，不是邻域的点数。 对滤波结果有影响的是标准差阈值系数k和邻域的点数。 使用的也是 nearestKSearch

缺点：对较远的激光线也筛除掉了。首先对激光雷达点云数据中的每个点而言，由于每个激光线束本身扫描频率是一致的， 显然越远的扫描线点会越稀疏，因此其平均距离会越远（即便其本身可能不一定是噪点）。因此更合理的方法可能是对每个点周围小范围内（同一条线内）的点进行高斯拟合，然后判断该点是否满足该高斯分布，但是如果需要得到比较准确的高斯分布参数，点数需要达到一定数量，否则和上述基于点云密度进行去噪差别不大。

#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/visualization/cloud_viewer.h>  //点云可视化相关定义
#include <pcl/filters/statistical_outlier_removal.h>  //滤波相关
#include <pcl/common/common.h>  

// 1.读取点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader r;
r.read<pcl::PointXYZ>("table_scene_lms400.pcd", *cloud);
cout << "there are " << cloud->points.size() << " points before filtering." << endl;

// 2.统计滤波
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);             // K近邻搜索点个数
sor.setStddevMulThresh(1.0); // 标准差倍数
sor.setNegative(false);     // 保留未滤波点（内点）
sor.filter(*cloud_filter);  // 保存滤波结果到cloud_filter

// 3.滤波结果保存
pcl::PCDWriter w;
w.writeASCII<pcl::PointXYZ>("table_scene_lms400_filter.pcd", *cloud_filter);
cout << "there are " << cloud_filter->points.size() << " points after filtering." << endl;

system("pause");
return 0;

常用滤波顺序: 直通 —-> 体素 —-> 统计

参考：统计滤波的源码