ransac.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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: ransac.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00040 
00041 #include "pcl/sample_consensus/ransac.h"
00042 
00044 template <typename PointT> bool
00045 pcl::RandomSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
00046 {
00047   // Warn and exit if no threshold was set
00048   if (threshold_ == DBL_MAX)
00049   {
00050     PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
00051     return (false);
00052   }
00053 
00054   iterations_ = 0;
00055   int n_best_inliers_count = -INT_MAX;
00056   double k = 1.0;
00057 
00058   std::vector<int> inliers;
00059   std::vector<int> selection;
00060   Eigen::VectorXf model_coefficients;
00061 
00062   int n_inliers_count = 0;
00063 
00064   // Iterate
00065   while (iterations_ < k)
00066   {
00067     // Get X samples which satisfy the model criteria
00068     sac_model_->getSamples (iterations_, selection);
00069 
00070     if (selection.empty ()) 
00071     {
00072       PCL_ERROR ("[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
00073       break;
00074     }
00075 
00076     // Search for inliers in the point cloud for the current plane model M
00077     if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
00078     {
00079       //++iterations_;
00080       continue;
00081     }
00082 
00083     // Select the inliers that are within threshold_ from the model
00084     sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);
00085     //if (inliers.empty () && k > 1.0)
00086     //  continue;
00087 
00088     n_inliers_count = inliers.size ();
00089 
00090     // Better match ?
00091     if (n_inliers_count > n_best_inliers_count)
00092     {
00093       n_best_inliers_count = n_inliers_count;
00094 
00095       // Save the current model/inlier/coefficients selection as being the best so far
00096       inliers_            = inliers;
00097       model_              = selection;
00098       model_coefficients_ = model_coefficients;
00099 
00100       // Compute the k parameter (k=log(z)/log(1-w^n))
00101       double w = (double)((double)n_best_inliers_count / (double)sac_model_->getIndices ()->size ());
00102       double p_no_outliers = 1.0 - pow (w, (double)selection.size ());
00103       p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
00104       p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
00105       k = log (1.0 - probability_) / log (p_no_outliers);
00106     }
00107 
00108     ++iterations_;
00109     if (debug_verbosity_level > 1)
00110       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);
00111     if (iterations_ > max_iterations_)
00112     {
00113       if (debug_verbosity_level > 0)
00114         PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
00115       break;
00116     }
00117   }
00118 
00119   if (debug_verbosity_level > 0)
00120     PCL_DEBUG ("[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), n_best_inliers_count);
00121 
00122   if (model_.empty ())
00123   {
00124     inliers_.clear ();
00125     return (false);
00126   }
00127 
00128   // Get the set of inliers that correspond to the best model found so far
00129   //sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
00130   return (true);
00131 }
00132 
00133 #define PCL_INSTANTIATE_RandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomSampleConsensus<T>;
00134 
00135 #endif    // PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_
00136