StatisticalOutlierRemoval源码

  1 *
  2  * Software License Agreement (BSD License)
  3  *
  4  *  Point Cloud Library (PCL) - www.pointclouds.org
  5  *  Copyright (c) 2010-2012, Willow Garage, Inc.
  6  *
  7  *  All rights reserved.
  8  *
  9  *  Redistribution and use in source and binary forms, with or without
 10  *  modification, are permitted provided that the following conditions
 11  *  are met:
 12  *
 13  *   * Redistributions of source code must retain the above copyright
 14  *     notice, this list of conditions and the following disclaimer.
 15  *   * Redistributions in binary form must reproduce the above
 16  *     copyright notice, this list of conditions and the following
 17  *     disclaimer in the documentation and/or other materials provided
 18  *     with the distribution.
 19  *   * Neither the name of the copyright holder(s) nor the names of its
 20  *     contributors may be used to endorse or promote products derived
 21  *     from this software without specific prior written permission.
 22  *
 23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 34  *  POSSIBILITY OF SUCH DAMAGE.
 35  *
 36  * $Id$
 37  *
 38  */
 39 
 40 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
 41 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
 42 
 43 #include <pcl/filters/statistical_outlier_removal.h>
 44 #include <pcl/common/io.h>
 45 
 46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 47 template <typename PointT> void
 48 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
 49 {
 50   std::vector<int> indices;
 51   if (keep_organized_)
 52   {
 53     bool temp = extract_removed_indices_;
 54     extract_removed_indices_ = true;
 55     applyFilterIndices (indices);
 56     extract_removed_indices_ = temp;
 57 
 58     output = *input_;
 59     for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
 60       output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
 61     if (!pcl_isfinite (user_filter_value_))
 62       output.is_dense = false;
 63   }
 64   else
 65   {
 66     applyFilterIndices (indices);
 67     copyPointCloud (*input_, indices, output);
 68   }
 69 }
 70 
 71 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 72 template <typename PointT> void
 73 pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
 74 {
 75   // Initialize the search class
 76   if (!searcher_)
 77   {
 78     if (input_->isOrganized ())
 79       searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
 80     else
 81       searcher_.reset (new pcl::search::KdTree<PointT> (false));
 82   }
 83   searcher_->setInputCloud (input_);
 84 
 85   // The arrays to be used
 86   std::vector<int> nn_indices (mean_k_);
 87   std::vector<float> nn_dists (mean_k_);
 88   std::vector<float> distances (indices_->size ());
 89   indices.resize (indices_->size ());
 90   removed_indices_->resize (indices_->size ());
 91   int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator
 92 
 93   // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
 94   int valid_distances = 0;
 95   for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
 96   {
 97     if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
 98         !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
 99         !pcl_isfinite (input_->points[(*indices_)[iii]].z))
100     {
101       distances[iii] = 0.0;
102       continue;
103     }
104 
105     // Perform the nearest k search
106     if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
107     {
108       distances[iii] = 0.0;
109       PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
110       continue;
111     }
112 
113     // Calculate the mean distance to its neighbors
114     double dist_sum = 0.0;
115     for (int k = 1; k < mean_k_ + 1; ++k)  // k = 0 is the query point
116       dist_sum += sqrt (nn_dists[k]);
117     distances[iii] = static_cast<float> (dist_sum / mean_k_);
118     valid_distances++;
119   }
120 
121   // Estimate the mean and the standard deviation of the distance vector
122   double sum = 0, sq_sum = 0;
123   for (size_t i = 0; i < distances.size (); ++i)
124   {
125     sum += distances[i];
126     sq_sum += distances[i] * distances[i];
127   }
128   double mean = sum / static_cast<double>(valid_distances);
129   double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
130   double stddev = sqrt (variance);
131   //getMeanStd (distances, mean, stddev);
132 
133   double distance_threshold = mean + std_mul_ * stddev;
134 
135   // Second pass: Classify the points on the computed distance threshold
136   for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
137   {
138     // Points having a too high average distance are outliers and are passed to removed indices
139     // Unless negative was set, then it‘s the opposite condition
140     if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
141     {
142       if (extract_removed_indices_)
143         (*removed_indices_)[rii++] = (*indices_)[iii];
144       continue;
145     }
146 
147     // Otherwise it was a normal point for output (inlier)
148     indices[oii++] = (*indices_)[iii];
149   }
150 
151   // Resize the output arrays
152   indices.resize (oii);
153   removed_indices_->resize (rii);
154 }
155 
156 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
157 
158 #endif  // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_