[学习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学习教程》

 

离群点删除(内外点渲染)[方法同上]

官方教程 http://pointclouds.org/documentation/tutorials/statistical_outlier.php#statistical-outlier-re

离群点是按照K个近邻点的标准方差*Threshold 来定义的，假如K=50,*Threshold =1. 那么某一个点是否是离群点。是这么确定的。首先求出这个点附近的50个点之间距离的标准方差dev，然后计算这个点到这些点的距离d，如果d>dev*threshold 那么这个点就是离群点。
附上源代码。在官方教程上加了一点点注释，并且把点云显示了一下，红色的是离群点。 点云文件上面的链接下载

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliner(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outliner(new pcl::PointCloud<pcl::PointXYZ>);
    // Fill in the cloud data
    pcl::PCDReader reader;
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZ>("table_scene_lms400.pcd", *cloud);

    std::cerr << "Cloud before filtering: " << std::endl;
    std::cerr << *cloud << std::endl;

    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(50);//50个临近点
    sor.setStddevMulThresh(1.0);//距离大于1倍标准方差

    sor.filter(*cloud_inliner);

    std::cerr << "Cloud after filtering: " << std::endl;
    std::cerr << *cloud_inliner << std::endl;

    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ>("table_scene_lms400_inliers.pcd", *cloud_inliner, false);

    sor.setNegative(true);
    sor.filter(*cloud_outliner);
    writer.write<pcl::PointXYZ>("table_scene_lms400_outliers.pcd", *cloud_outliner, false);
    pcl::visualization::PCLVisualizer viewer("demo");
    int v1(0);
    int v2(1);
    viewer.createViewPort(0.0, 0.0, 1.0, 1.0, v1);
    // The color we will be using
    float bckgr_gray_level = 0.0;  // Black
    float txt_gray_lvl = 1.0 - bckgr_gray_level;

    // Original point cloud is white
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h(cloud, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);
    viewer.addPointCloud(cloud, cloud_in_color_h, "cloud_in_v1", v1);       //viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_out_green(cloud_inliner, 20, 180, 20);
    viewer.addPointCloud(cloud_inliner, cloud_out_green, "cloud_out", v1);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> Final_red(cloud_outliner, 180, 10, 20);
    viewer.addPointCloud(cloud_outliner, Final_red, "cloud_Final", v1);
        viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
    //  viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);     
    viewer.setSize(1280, 1024);  // Visualiser window size
    //viewer.showCloud(cloud_out);
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }

    return (0);
}

 

PCL 之离群点去除

基于半径（规定圆内和邻居个数限制）和条件（滤波域和值范围）实行离群点去除

/*PCL 之离群点去除:基于半径（规定圆内和邻居个数限制）和条件（滤波域和值范围）实行离群点去除.
https://blog.csdn.net/zhaoxr233/article/details/91997811*/


#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/io/ply_io.h>
#include<pcl/filters/radius_outlier_removal.h>
#include<pcl/filters/conditional_removal.h>
#include <pcl/visualization/cloud_viewer.h>  //点云可视化相关定义
#include <pcl/visualization/pcl_visualizer.h>
using namespace std;
int main(int argc, char** argv){
    if (argc != 3){
        cerr << "please specify commond line arg f.pcd -r or -c" << endl;
        exit(0);
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    //读入数据
    pcl::PCDReader pcdr;
    pcdr.read("table_scene_lms400.pcd", *cloud);
//    pcdr.read<pcl::PointXYZ>(argv[1],*cloud);
    //Fill in the cloud data
    /*cloud->width = 10;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
*/
    if (strcmp(argv[2], "-r") == 0){
        //基于半径的离群点剔除
        pcl::RadiusOutlierRemoval<pcl::PointXYZ>  rout;
        rout.setInputCloud(cloud);
        rout.setRadiusSearch(0.01);//设置搜索半径的值
        rout.setMinNeighborsInRadius(15);//设置最小邻居个数，默认是1
        rout.filter(*filtered_cloud);
    }
    else if (strcmp(argv[2],"-c")==0)
    {
        //条件滤波器去除离群点
        pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_c(new pcl::ConditionAnd<pcl::PointXYZ>());//初始化条件滤波对象
        range_c->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, -1.8)));//设定滤波条件z轴 GT表示greater than  LT表示less than
        range_c->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 1.8)));
        pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
        condrem.setCondition(range_c);
        condrem.setInputCloud(cloud);
        condrem.setKeepOrganized(true);//保持点云数据的结构
        condrem.filter(*filtered_cloud);
    }
    else{
        cerr << "please specify commond line arg f.pcd -r or -c" << endl;
        exit(0);
    }

    //可视化f前
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer( new pcl::visualization::PCLVisualizer ("Viewer"));
    viewer->addPointCloud(filtered_cloud, "cloud");

    while(!viewer->wasStopped()){
        viewer->spinOnce();
    }

    cerr << "the size of cloud before filtering "<<argv[1]<<" : " << cloud->points.size() << endl;
    cerr << "the size of cloud after filtering " << argv[1] << " : " << filtered_cloud->points.size() << endl;
    pcl::PLYWriter plyw;
    plyw.write("origin_table.ply", *cloud);
    plyw.write("r_filtered_cloud.ply", *filtered_cloud); //根据方法的不同修改文件名
    return 0;
}