ransac.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) 2009, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00042 #define PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00043 
00044 #include <pcl/sample_consensus/ransac.h>
00045 
00047 template <typename PointT> bool
00048 pcl::RandomSampleConsensus<PointT>::computeModel (int)
00049 {
00050   // Warn and exit if no threshold was set
00051   if (threshold_ == std::numeric_limits<double>::max())
00052   {
00053     PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
00054     return (false);
00055   }
00056 
00057   iterations_ = 0;
00058   int n_best_inliers_count = -INT_MAX;
00059   double k = 1.0;
00060 
00061   std::vector<int> selection;
00062   Eigen::VectorXf model_coefficients;
00063 
00064   double log_probability  = log (1.0 - probability_);
00065   double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
00066 
00067   int n_inliers_count = 0;
00068   unsigned skipped_count = 0;
00069   // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
00070   const unsigned max_skip = max_iterations_ * 10;
00071   
00072   // Iterate
00073   while (iterations_ < k && skipped_count < max_skip)
00074   {
00075     // Get X samples which satisfy the model criteria
00076     sac_model_->getSamples (iterations_, selection);
00077 
00078     if (selection.empty ()) 
00079     {
00080       PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
00081       break;
00082     }
00083 
00084     // Search for inliers in the point cloud for the current plane model M
00085     if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
00086     {
00087       //++iterations_;
00088       ++skipped_count;
00089       continue;
00090     }
00091 
00092     // Select the inliers that are within threshold_ from the model
00093     //sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
00094     //if (inliers.empty () && k > 1.0)
00095     //  continue;
00096 
00097     n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
00098 
00099     // Better match ?
00100     if (n_inliers_count > n_best_inliers_count)
00101     {
00102       n_best_inliers_count = n_inliers_count;
00103 
00104       // Save the current model/inlier/coefficients selection as being the best so far
00105       model_              = selection;
00106       model_coefficients_ = model_coefficients;
00107 
00108       // Compute the k parameter (k=log(z)/log(1-w^n))
00109       double w = static_cast<double> (n_best_inliers_count) * one_over_indices;
00110       double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
00111       p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
00112       p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
00113       k = log_probability / log (p_no_outliers);
00114     }
00115 
00116     ++iterations_;
00117     PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n", iterations_, k, n_inliers_count, n_best_inliers_count);
00118     if (iterations_ > max_iterations_)
00119     {
00120       PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
00121       break;
00122     }
00123   }
00124 
00125   PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_best_inliers_count);
00126 
00127   if (model_.empty ())
00128   {
00129     inliers_.clear ();
00130     return (false);
00131   }
00132 
00133   // Get the set of inliers that correspond to the best model found so far
00134   sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
00135   return (true);
00136 }
00137 
00138 #define PCL_INSTANTIATE_RandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomSampleConsensus<T>;
00139 
00140 #endif    // PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00141 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:01