目录

1. 统计滤波原理

2. PCL统计滤波源码详解

1. 统计滤波原理

      统计滤波假设所有点的 “k 近邻平均距离值”符合正态分布，通过判断某点的“k 近邻平均距离值”是否在设置区间内，以判别该点是否为离群点或噪点。

      详细算法过程：
      1）计算输入点云所有点的“k 近邻平均距离值” k-distances【符合正态分布】；
      2）计算 k-distances 的平均值 mean 和标准差 σ；
      3）计算统计距离区间，一般为[mean - k*σ，mean + k*σ]，k为自定义系数，进行点云分类。

      看PCL源码，步骤3是 d > mean + k*σ  即为噪点。这一块还要再核对下。

2. PCL统计滤波源码详解

      PCL实现统计滤波算法的主要接口为 applyFilterIndices 

      源码路径：GitCode - 全球开发者的开源社区,开源代码托管平台

template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices(Indices& indices)
{
    // 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));
    }
	
	// 构建kdtree
	if (!searcher_->setInputCloud (input_)){
		PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ());
		indices.clear ();
		removed_indices_->clear ();
		return;
    }
	
	// The arrays to be used
    const int searcher_k = mean_k_ + 1;                // mean_k_值要+1，因为近邻搜寻结果包含query点
    Indices nn_indices (searcher_k);
    std::vector<float> nn_dists (searcher_k);
    std::vector<float> distances (indices_->size ());  // 保存每个点对应的K近邻平均距离
    indices.resize (indices_->size ());
    removed_indices_->resize (indices_->size ());
    int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator


    // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
	// 第一步：使用for循环【串行】计算每个点的K近邻平均距离，iii = input indices iterator
    int valid_distances = 0;
    for (int iii = 0; iii < static_cast<int> (indices_->size()); ++iii){
        // 数据有效性判断
        if (!std::isfinite((*input_)[(*indices_)[iii]].x) ||
            !std::isfinite((*input_)[(*indices_)[iii]].y) ||
            !std::isfinite((*input_)[(*indices_)[iii]].z)){
            distances[iii] = 0.0;
            continue;
        }

        // Perform the nearest k search
        // k近邻搜寻
		if (searcher_->nearestKSearch((*indices_)[iii], searcher_k, 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
        // 计算当前点对应的K近邻平均距离；k = 0是当前点，不参与计算
        double dist_sum = 0.0;
        for (std::size_t k = 1; k < nn_dists.size(); ++k)
            dist_sum += sqrt((nn_dists[k]);
        distances[iii] = static_cast<float>(dist_sum / (nn_dists.size() - 1));
        valid_distances++;
    }


    // Estimate the mean and the standard deviation of the distance vector
    // 计算平均值和标准差
    double sum = 0, sq_sum = 0;
    for (const float& distance : distances){
        sum += distance;
        sq_sum += distance * distance;
    }
    // 平均值 和 方差
    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;

    // Second pass: Classify the points on the computed distance threshold
    // 基于计算的距离阈值，将点云进行分类； // iii = input indices iterator
    for (int iii = 0; iii < static_cast<int> (indices_->size()); ++iii) {
        // 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
        // 大于距离阈值的点视为局外点，negative设置为TRUE，则情况相反
        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);
}