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