StatisticalOutlierRemoval源码

源代码

 *
  * Software License Agreement (BSD License)
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) -, Willow Garage, Inc.
  *
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
  *  modification, are permitted provided that the following conditions
  *  are met:
  *
  *   * Redistributions of source code must retain the above copyright
  *     notice, this list of conditions and the following disclaimer.
  *   * Redistributions in binary form must reproduce the above
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
  *   * Neither the name of the copyright holder(s) nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *
  * $Id$
  *
  */

 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_

 #include <pcl/filters/statistical_outlier_removal.h>
 #include <pcl/common/io.h>

 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
 {
   std::vector<int> indices;
   if (keep_organized_)
   {
     bool temp = extract_removed_indices_;
     extract_removed_indices_ = true;
     applyFilterIndices (indices);
     extract_removed_indices_ = temp;

     output = *input_;
     for (int rii = ; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
       output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
     if (!pcl_isfinite (user_filter_value_))
       output.is_dense = false;
   }
   else
   {
     applyFilterIndices (indices);
     copyPointCloud (*input_, indices, output);
   }
 }

 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &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));
   }
   searcher_->setInputCloud (input_);

   // 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 = , rii = ;  // oii = output indices iterator, rii = removed indices iterator

   // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
   int valid_distances = ;
   for (int iii = ; 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_ + , nn_indices, nn_dists) == )
     {
       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 = ; k < mean_k_ + ; ++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 = , sq_sum = ;
   for (size_t i = ; 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) - );
   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
   for (int iii = ; 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);
 }

 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;

 #endif  // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_

最终会执行

template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)

1、进行一些简单Initialize

// 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));
  }
  searcher_->setInputCloud (input_);

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

　3、求每个点的k邻域的均值

  // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
  int valid_distances = ;
  for (int iii = ; 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_ + , nn_indices, nn_dists) == )
    {
      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 = ; k < mean_k_ + ; ++k)  // k = 0 is the query point
      dist_sum += sqrt (nn_dists[k]);
    distances[iii] = static_cast<float> (dist_sum / mean_k_);//每个点都对应了一个距离变量
    valid_distances++;
  }

4、估计距离的均值和标准差   不是邻域 ，是根据整个数据中的点均值和标准差

// Estimate the mean and the standard deviation of the distance vector
  double sum = , sq_sum = ;
  for (size_t i = ; 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);

5、根据设定的距离阈值与distances[iii]比较 ，超出设定阈值则该点被标记为离群点，并将其移除。

　

double distance_threshold = mean + std_mul_ * stddev;

  // Second pass: Classify the points on the computed distance threshold
  for (int iii = ; 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];
  }

indices.resize (oii);
    removed_indices_->resize (rii);// Resize the output arrays