pcl源码分析之统计滤波

---

前言

统计滤波对于处理点云杂点有着广泛的用途，在此将pcl的统计滤波源码分析总结一下。

---

一、调用统计滤波

调用统计滤波，在我之前的博文里已经写的很清楚了。

二、统计滤波源码分析

1.源码分析

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_);
  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;
  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
    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
    //计算每个点到所有K邻域点的平均距离
    //从这里可以看出setMeanK(sum)中的sum就是mean_k_，也就是我们设置的最近邻点的个数 ，值越大，计算效率越慢
    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++;
  }

  // Estimate the mean and the standard deviation of the distance vector
  //计算整个点集距离容器的平均值和样本标准差
  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);//求标准差（均方差）
  //getMeanStd (distances, mean, stddev);
  
  //距离阈值等于平均距离加上标准差倍数
  //从这里可以看出来，为什么在调用时，设置的阈值越小，滤波效果越明显
  double distance_threshold = mean + std_mul_ * stddev;

  // Second pass: Classify the points on the computed distance threshold
  //依次将距离阈值与每个点的distances比较 ，超出阈值的点被标记为离群点，并将其移除。
  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);
}

可以回顾下方差，标准差的概念。

2.整体流程

1. 计算整个点云每个点到其K邻域点的平均距离；
2. 计算整个点云距离容器的平均值和样本标准差；
3. 依次将距离阈值与每个点的distances比较 ，超出阈值的点被标记为离群点，并将其移除。

---

总结

分析了下pcl统计滤波这块的源码，了解其原理，在调用时方便设置参数。