統計濾波

1.原理介紹

StatisticalOutlierRemoval濾波器主要可以用來剔除離群點，或者測量誤差導致的粗差點。
濾波思想為：對每一個點的鄰域進行一個統計分析，計算它到所有臨近點的平均距離。假設得到的結果是一個高斯分布，其形狀是由均值和標準差決定，那麼平均距離在標準範圍(由全局距離平均值和方差定義)之外的點，可以被定義為離群點並從數據中去除。

2.源碼剖析

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;  
第一步：計算每個點到所有K鄰域點的平均距離。
int valid_distances = 0;for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  {  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;  }
    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;  }
    double dist_sum = 0.0;  for (int k = 1; k < mean_k_ + 1; ++k)      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 vectordouble 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]比較 ，超出閾值的點被標記為離群點，並將其移除。
    for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)    {            if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))    {      if (extract_removed_indices_)        (*removed_indices_)[rii++] = (*indices_)[iii];      continue;    }
        indices[oii++] = (*indices_)[iii];  }
    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(){        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;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;    sor.setInputCloud(cloud);    sor.setMeanK(50);     sor.setStddevMulThresh(1.0);     sor.setNegative(false);     sor.filter(*cloud_filter);  
        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.示例代碼結果
統計濾波前
統計濾波後