PCL中的半径离群点去噪-RadiusOutlierRemoval

该滤波器会计算每个点周围一定半径内点的密度，并将密度低于一个阈值的点移除。

template <typename PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
  //首先检查search_radius_是否为0，如果是，则输出错误信息，并清空indices和removed_indices_，然后返回。
  if (search_radius_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
    indices.clear ();
    removed_indices_->clear ();
    return;
  }

  //如果searcher_为空指针，则根据输入的点云是否有组织结构来选择初始化OrganizedNeighbor或者KdTree的搜索类，并将输入点云的指针传递给搜索类。
  // 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 (indices_->size ());
  std::vector<float> nn_dists (indices_->size ());
  indices.resize (indices_->size ());
  removed_indices_->resize (indices_->size ());
  int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator

  // If the data is dense => use nearest-k search
  if (input_->is_dense)
  {
    // Note: k includes the query point, so is always at least 1
    int mean_k = min_pts_radius_ + 1;
    double nn_dists_max = search_radius_ * search_radius_;

    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
    {
      //采用最近K邻搜索。对于每个输入点，在搜索半径内找到最近的K个点，然后根据条件判断是否将该点加入输出索引或移除索引。
      // Perform the nearest-k search
      int k = searcher_->nearestKSearch (*it, mean_k, nn_indices, nn_dists);

      // Check the number of neighbors
      // Note: nn_dists is sorted, so check the last item
      bool chk_neighbors = true;
      if (k == mean_k)
      {
        if (negative_)
        {
          chk_neighbors = false;
          if (nn_dists_max < nn_dists[k-1])
          {
            chk_neighbors = true;
          }
        }
        else
        {
          chk_neighbors = true;
          if (nn_dists_max < nn_dists[k-1])
          {
            chk_neighbors = false;
          }
        }
      }
      else
      {
        if (negative_)
          chk_neighbors = true;
        else
          chk_neighbors = false;
      }

      // Points having too few neighbors are outliers and are passed to removed indices
      // Unless negative was set, then it's the opposite condition
      if (!chk_neighbors)
      {
        if (extract_removed_indices_)
          (*removed_indices_)[rii++] = *it;
        continue;
      }

      // Otherwise it was a normal point for output (inlier)
      indices[oii++] = *it;
    }
  }
  // NaN or Inf values could exist => use radius search
  else
  {
    for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
    {
      //采用半径搜索。对于每个输入点，在搜索半径内找到点，然后根据条件判断是否将该点加入输出索引或移除索引。最后，调整输出数组的大小。
      // Perform the radius search
      // Note: k includes the query point, so is always at least 1
      int k = searcher_->radiusSearch (*it, search_radius_, nn_indices, nn_dists);

      // Points having too few neighbors are outliers and are passed to removed indices
      // Unless negative was set, then it's the opposite condition
      if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
      {
        if (extract_removed_indices_)
          (*removed_indices_)[rii++] = *it;
        continue;
      }

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

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