Point Cloud Library (PCL)  1.3.1
statistical_outlier_removal.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: statistical_outlier_removal.hpp 3028 2011-11-01 04:12:17Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00039 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00040 
00041 #include "pcl/filters/statistical_outlier_removal.h"
00042 
00043 
00045 template <typename PointT> void
00046 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
00047 {
00048   if (std_mul_ == 0.0)
00049   {
00050     PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multiplier not set!\n", getClassName ().c_str ());
00051     output.width = output.height = 0;
00052     output.points.clear ();
00053     return;
00054   }
00055 
00056   if (input_->points.empty ())
00057   {
00058     output.width = output.height = 0;
00059     output.points.clear ();
00060     return;
00061   }
00062 
00063   // Initialize the spatial locator
00064   if (!tree_)
00065   {
00066     if (input_->isOrganized ())
00067       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00068     else
00069       tree_.reset (new pcl::search::KdTree<PointT> (false));
00070   }
00071 
00072   // Send the input dataset to the spatial locator
00073   tree_->setInputCloud (input_);
00074 
00075   // Allocate enough space to hold the results
00076   std::vector<int> nn_indices (mean_k_);
00077   std::vector<float> nn_dists (mean_k_);
00078 
00079   std::vector<float> distances (indices_->size ());
00080   // Go over all the points and calculate the mean or smallest distance
00081   for (size_t cp = 0; cp < indices_->size (); ++cp)
00082   {
00083     if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
00084         !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
00085         !pcl_isfinite (input_->points[(*indices_)[cp]].z))
00086     {
00087       distances[cp] = 0;
00088       continue;
00089     }
00090 
00091     if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0)
00092     {
00093       distances[cp] = 0;
00094       PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
00095       continue;
00096     }
00097 
00098     // Minimum distance (if mean_k_ == 2) or mean distance
00099     double dist_sum = 0;
00100     for (int j = 1; j < mean_k_; ++j)
00101       dist_sum += sqrt (nn_dists[j]);
00102     distances[cp] = dist_sum / (mean_k_-1);
00103   }
00104 
00105   // Estimate the mean and the standard deviation of the distance vector
00106   double mean, stddev;
00107   getMeanStd (distances, mean, stddev);
00108   double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
00109 
00110   output.points.resize (input_->points.size ());      // reserve enough space
00111   removed_indices_->resize (input_->points.size ());
00112   
00113   // Build a new cloud by neglecting outliers
00114   int nr_p = 0;
00115   int nr_removed_p = 0;
00116   
00117   for (size_t cp = 0; cp < indices_->size (); ++cp)
00118   {
00119     if (negative_)
00120     {
00121       if (distances[cp] <= distance_threshold)
00122       {
00123         if (extract_removed_indices_)
00124         {
00125           (*removed_indices_)[nr_removed_p] = cp;
00126           nr_removed_p++;
00127         }
00128         continue;
00129       }
00130     }
00131     else
00132     {
00133       if (distances[cp] > distance_threshold)
00134       {
00135         if (extract_removed_indices_)
00136         {
00137           (*removed_indices_)[nr_removed_p] = cp;
00138           nr_removed_p++;
00139         }
00140         continue;
00141       }
00142     }
00143 
00144     output.points[nr_p++] = input_->points[(*indices_)[cp]];
00145   }
00146 
00147   output.points.resize (nr_p);
00148   output.width  = nr_p;
00149   output.height = 1;
00150   output.is_dense = true; // nearestKSearch filters invalid points
00151 
00152   removed_indices_->resize (nr_removed_p);
00153 }
00154 
00155 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
00156 
00157 #endif    // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
00158 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines