PCL滤波工具之StatisticalOutlierRemoval深度分析

PCL中用于点云滤波的一个工具是StatisticalOutlierRemoval

其大致的原理思路是：

1. 对每个点，我们计算它到它的K邻域内所有点的平均距离d。对输入点云中的每个点都进行这样的计算，即每个点都可以求得其对应的d，因此可以得到一个包含各点d值的数组，记为distance。

2. 对于输入点云中的所有点，假设得到distance数组中的各元素构成一个高斯分布，该数组即为一个样本，样本容量为点云包含的点数目。高斯分布曲线的形状由样本的均值和标准差决定，d值在标准范围（由样本的均值和方差定义〉之外的对应点，可被定义为离群点并可从数据集中去除掉。

 具体的，下面贴上PCL源码进行分析

//该函数包含了上述算法的主要操作
template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
  // Initialize the search class —— —— 初始化
  if (!searcher_)
  {
    if (input_->isOrganized ())
      searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
    else
      searcher_.reset (new pcl::search::KdTree<PointT> (false));
  }
  searcher_->setInputCloud (input_);

  // The arrays to be used
  std::vector<int> nn_indices (mean_k_); //mean_k_为定义的K邻域搜索的K值
  std::vector<float> nn_dists (mean_k_);
  std::vector<float> distances (indices_->size ());
  indices.resize (indices_->size ());
  removed_indices_->resize (indices_->size ());
  int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator

  // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
  int valid_distances = 0; //记录共有多少点
  //下面开始遍历点云中各点，计算各点到其K邻域点的平均距离，每个点的结果放入distance数组中
  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
  {
    if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
        !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
        !pcl_isfinite (input_->points[(*indices_)[iii]].z))
    {
      distances[iii] = 0.0;
      continue;
    }

    // Perform the nearest k search
    //searcher_->nearestKSearch函数是PCL中定义的K邻域搜索模块中的函数
    //searcher_->nearestKSearch有四个参数：
    //para1：查找点； para2：搜索范围K+1；
    //para3：k个邻域点的下标；
    //para4：k个邻域点到该查找点的距离的平方（是两点之间距离的平方）
    if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
    {
      distances[iii] = 0.0;
      PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
      continue;
    }

    // Calculate the mean distance to its neighbors
    double dist_sum = 0.0;
    for (int k = 1; k < mean_k_ + 1; ++k)  // k = 0 is the query point
      dist_sum += sqrt (nn_dists[k]);
    distances[iii] = static_cast<float> (dist_sum / mean_k_);
    valid_distances++;
  } //结束循环，得到点云中各点对应的K邻域平均距离

  // Estimate the mean and the standard deviation of the distance vector
  //计算distance数组的均值mean和标准偏差stddev——反映了整片点云的性质
  double sum = 0, sq_sum = 0;
  for (size_t i = 0; i < distances.size (); ++i)
  {
    sum += distances[i];
    sq_sum += distances[i] * distances[i];
  }
  double mean = sum / static_cast<double>(valid_distances);
  double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
  double stddev = sqrt (variance);
  

  //基于上述计算得到的整片点云的mean和stddev，设置判断阈值distance_threshold
  //distance_threshold是用于判断点云中点是否为离群点的阈值
  double distance_threshold = mean + std_mul_ * stddev;

  //比较各点的k邻域平均距离与整片点云的distance_threshold，若超过阈值则作为离群点舍弃
  // Second pass: Classify the points on the computed distance threshold
  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
  {
    // Points having a too high average distance are outliers and are passed to removed indices
    // Unless negative was set, then it's the opposite condition
    if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
    {
      if (extract_removed_indices_)
        (*removed_indices_)[rii++] = (*indices_)[iii];
      continue;
    }

    // Otherwise it was a normal point for output (inlier)
    indices[oii++] = (*indices_)[iii];
  }

  // Resize the output arrays
  indices.resize (oii);
  removed_indices_->resize (rii);
}

 通过代码中的细节可以看到，

1. 对每个点，计算它到它的K邻域内所有点的平均距离d，最终放到数组distance中；

2. 数组distance所构成的样本，其mean均值计算方式就是简单的元素相加除以样本数量

3. 数组distance所构成的样本，其stddev标准偏差（standard deviation）计算方式是：

for (size_t i = 0; i < distances.size (); ++i)
  {
    sum += distances[i];
    sq_sum += distances[i] * distances[i];
  }
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);

double stddev = sqrt (variance);

4. 作为离群点判断的阈值distance_threshold，其计算方式是：

double distance_threshold = mean + std_mul_ * stddev;

其中，std_mul_是一个标准偏差的倍乘参数，用于调整阈值大小，在使用PCL的StatisticalOutlierRemoval类时，该参数std_mul_由用户设置

5.关于std_mul_的选取：由于只有当某个点的K邻域平均距离大于该阈值distance_threshold 时才会被认定为离群点，而且distance_threshold = mean + std_mul_ * stddev。又因为对于同一片点云，其mean和stddev是不变的，因此std_mul_ 的大小决定了滤波的效果。

std_mul_越小，则distance_threshold 越小，则过滤效果越明显（大于阈值的点被过滤）

反之，std_mul_越大，则distance_threshold 越大，则过滤效果越不明显

至于选取std_mul_时是否需要考虑其对应的几何意义，本人认为不需要，只需要遵循上面规律，根据滤波测试结果对std_mul_参数调试选择最合适的即可，似乎不好根据几何意义选取std_mul_的值。

在上述分析后，进行一个实际的测试。使用PCL的StatisticalOutlierRemoval类的代码如下（需要使用头文件：#include <pcl/filters/statistical_outlier_removal.h>）：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   //创建滤波器对象
sor.setInputCloud(cloud);                           //设置待滤波的点云
sor.setMeanK(50);                               //设置在进行统计时考虑的临近点个数
sor.setStddevMulThresh(1.0);                      //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
sor.filter(*cloud_filtered);                    //滤波结果存储到cloud_filtered

对输入的一片点云进行滤波，设置std_mul_值分别为2.0，1.0，结果如下：

std_mul_=2.0时，初始点有14722个，过滤后还剩14043个，滤掉了679个，占原有点的4.6%；

std_mul_=1.0时，过滤后还剩13125个，滤掉了1597个，占原有点的10.8%；

 最后，对PCL的StatisticalOutlierRemoval类进行了简单分析：

该滤波方法的实质为，对查找点到其K邻域点的平均距离这一值进行判断。判断的依据（即阈值）与全局点的分布相关，但是对于一片点云来说，该阈值是固定不变的。因此，某查找点的邻域点分布情况，就决定了它是否容易被认定为离群点。

因此，容易被该方法认定为离群点过滤掉的点，一般是以下几类点：

1. 相对孤单的离群点，由于是离群的，其K邻域的平均距离会比较大，因此会被认定为离群点——这是滤波算法想要的结果。但是，有的情况是这样的，离群的这些点具有聚集性，例如一大片平面点外，有一小簇（不只有几个点）噪声点，为了将这部分离群点去除，需要设置的K邻域搜索范围K足够大（应该要比这一簇噪声点的包含点数尽量大一些），这样就可以去除这部分离群点。不过随之带来的问题是，K搜索范围增大会减慢运行速度。

2. 边缘点。边缘点的K邻域不像非边缘点，其K近邻点的平均距离肯定是要大一些的，因此上述PCL的StatisticalOutlierRemoval类在过滤离群点时，也会将点云的边缘的点过滤一部分——这是算法所不想要的结果。

所以，该方法存在一些弊端，一是对于离群点具有小范围聚集的情况，算法为保证过滤效果需要更大的耗时；二是在过滤离群点的同时，会把边界点过滤。