statistical_outlier_removal.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00041 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00042 
00043 #include <pcl/filters/statistical_outlier_removal.h>
00044 #include <pcl/common/io.h>
00045 
00047 template <typename PointT> void
00048 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
00049 {
00050   std::vector<int> indices;
00051   if (keep_organized_)
00052   {
00053     bool temp = extract_removed_indices_;
00054     extract_removed_indices_ = true;
00055     applyFilterIndices (indices);
00056     extract_removed_indices_ = temp;
00057 
00058     output = *input_;
00059     for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
00060       output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
00061     if (!pcl_isfinite (user_filter_value_))
00062       output.is_dense = false;
00063   }
00064   else
00065   {
00066     applyFilterIndices (indices);
00067     copyPointCloud (*input_, indices, output);
00068   }
00069 }
00070 
00072 template <typename PointT> void
00073 pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
00074 {
00075   // Initialize the search class
00076   if (!searcher_)
00077   {
00078     if (input_->isOrganized ())
00079       searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00080     else
00081       searcher_.reset (new pcl::search::KdTree<PointT> (false));
00082   }
00083   searcher_->setInputCloud (input_);
00084 
00085   // The arrays to be used
00086   std::vector<int> nn_indices (mean_k_);
00087   std::vector<float> nn_dists (mean_k_);
00088   std::vector<float> distances (indices_->size ());
00089   indices.resize (indices_->size ());
00090   removed_indices_->resize (indices_->size ());
00091   int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator
00092 
00093   // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
00094   int valid_distances = 0;
00095   for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
00096   {
00097     if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
00098         !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
00099         !pcl_isfinite (input_->points[(*indices_)[iii]].z))
00100     {
00101       distances[iii] = 0.0;
00102       continue;
00103     }
00104 
00105     // Perform the nearest k search
00106     if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
00107     {
00108       distances[iii] = 0.0;
00109       PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
00110       continue;
00111     }
00112 
00113     // Calculate the mean distance to its neighbors
00114     double dist_sum = 0.0;
00115     for (int k = 1; k < mean_k_ + 1; ++k)  // k = 0 is the query point
00116       dist_sum += sqrt (nn_dists[k]);
00117     distances[iii] = static_cast<float> (dist_sum / mean_k_);
00118     valid_distances++;
00119   }
00120 
00121   // Estimate the mean and the standard deviation of the distance vector
00122   double sum = 0, sq_sum = 0;
00123   for (size_t i = 0; i < distances.size (); ++i)
00124   {
00125     sum += distances[i];
00126     sq_sum += distances[i] * distances[i];
00127   }
00128   double mean = sum / static_cast<double>(valid_distances);
00129   double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
00130   double stddev = sqrt (variance);
00131   //getMeanStd (distances, mean, stddev);
00132 
00133   double distance_threshold = mean + std_mul_ * stddev;
00134 
00135   // Second pass: Classify the points on the computed distance threshold
00136   for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
00137   {
00138     // Points having a too high average distance are outliers and are passed to removed indices
00139     // Unless negative was set, then it's the opposite condition
00140     if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
00141     {
00142       if (extract_removed_indices_)
00143         (*removed_indices_)[rii++] = (*indices_)[iii];
00144       continue;
00145     }
00146 
00147     // Otherwise it was a normal point for output (inlier)
00148     indices[oii++] = (*indices_)[iii];
00149   }
00150 
00151   // Resize the output arrays
00152   indices.resize (oii);
00153   removed_indices_->resize (rii);
00154 }
00155 
00156 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
00157 
00158 #endif  // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00159 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:33:54