PCL系列7——半径滤波(离群点剔除)

1.原理介绍

点云半径滤波也叫基于连通分析的点云滤波，该方法的基本思想是假定原始点云中每个激光点在指定的半径邻域中至少包含一定数量的近邻点。原始点云中符合假设条件的激光点被视为正常点进行保留，反之，则视为噪声点并进行去除。该方法对原始激光点云中存在的一些悬空的孤立点或无效点具有很好的去除效果。

2.源码剖析

template <typename PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
  if (search_radius_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
    indices.clear ();
    removed_indices_->clear ();
    return;
  }

  // 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)
    {
      // 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);
}

3.示例代码

#include   //文件输入输出
#include   //点类型相关定义
#include   //点云可视化相关定义
#include   //滤波相关
#include   
#include 

using namespace std;

int main()
{
	//1.读取点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader r;
	r.read<pcl::PointXYZ>("data\\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::RadiusOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);
	sor.setRadiusSearch(0.02);
	sor.setMinNeighborsInRadius(15);
	sor.setNegative(false); 
	sor.filter(*cloud_filter);  

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

	system("pause");
	return 0;
}

4.示例代码结果

PCL官网示例