PCL系列6——统计滤波

PCL库中的统计滤波使用示例

1.原理介绍

StatisticalOutlierRemoval滤波器主要可以用来剔除离群点，或者测量误差导致的粗差点。
滤波思想为：对每一个点的邻域进行一个统计分析，计算它到所有临近点的平均距离。假设得到的结果是一个高斯分布，其形状是由均值和标准差决定，那么平均距离在标准范围(由全局距离平均值和方差定义)之外的点，可以被定义为离群点并从数据中去除。

2.源码剖析

// 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

第一步：计算每个点到所有K邻域点的平均距离。

//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
  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;

第三步：依次将距离阈值与每个点的distances[iii]比较 ，超出阈值的点被标记为离群点，并将其移除。

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

3.示例代码

#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>  

#include <iostream>
#include <vector>

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::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>("data\\table_scene_lms400_filter.pcd", *cloud_filter);
	cout << "there are " << cloud_filter->points.size() << " points after filtering." << endl;

	system("pause");
	return 0;
}

4.示例代码结果

参考

《点云库PCL学习教程》