semantic_point_annotator: semantic_point_annotator: ransac.cpp Source File

00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: ransac.cpp 16379 2009-05-29 19:20:46Z hsujohnhsu $
00028  *
00029  */
00030 
00033 #include <limits>
00034 #include <ransac.h>
00035 
00036 namespace sample_consensus
00037 {
00039 
00043   RANSAC::RANSAC (SACModel *model, double threshold) : SAC (model)
00044   {
00045     this->threshold_ = threshold;
00046     // Desired probability of choosing at least one sample free from outliers
00047     this->probability_    = 0.99;
00048     // Maximum number of trials before we give up.
00049     this->max_iterations_ = 10000;
00050 
00051     this->iterations_ = 0;
00052   }
00053 
00055 
00058   RANSAC::RANSAC (SACModel* model) : SAC (model) { }
00059 
00061 
00064   bool
00065     RANSAC::computeModel (int debug)
00066   {
00067     iterations_ = 0;
00068     int n_best_inliers_count = -INT_MAX;
00069     double k = 1.0;
00070 
00071     std::vector<int> best_model;
00072     std::vector<int> best_inliers, inliers;
00073     std::vector<int> selection;
00074 
00075     int n_inliers_count = 0;
00076 
00077     // Iterate
00078     while (iterations_ < k)
00079     {
00080       // Get X samples which satisfy the model criteria
00081       sac_model_->getSamples (iterations_, selection);
00082 
00083       if (selection.size () == 0) break;
00084 
00085       // Search for inliers in the point cloud for the current plane model M
00086       sac_model_->computeModelCoefficients (selection);
00087 
00088       sac_model_->selectWithinDistance (sac_model_->getModelCoefficients (), threshold_, inliers);
00089       n_inliers_count = inliers.size ();
00090 
00091       // Better match ?
00092       if (n_inliers_count > n_best_inliers_count)
00093       {
00094         n_best_inliers_count = n_inliers_count;
00095         best_inliers = inliers;
00096         //inliers.clear ();
00097         best_model = selection;
00098 
00099         // Compute the k parameter (k=log(z)/log(1-w^n))
00100         double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ());
00101         double p_no_outliers = 1 - pow (w, (double)selection.size ());
00102         p_no_outliers = std::max (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
00103         p_no_outliers = std::min (1 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
00104         k = log (1 - probability_) / log (p_no_outliers);
00105       }
00106 
00107       iterations_ += 1;
00108       if (debug > 1)
00109         std::cerr << "[RANSAC::computeModel] Trial " << iterations_ << " out of " << ceil (k) << ": " << n_inliers_count << " inliers (best is: " << n_best_inliers_count << " so far)." << std::endl;
00110       if (iterations_ > max_iterations_)
00111       {
00112         if (debug > 0)
00113           std::cerr << "[RANSAC::computeModel] RANSAC reached the maximum number of trials." << std::endl;
00114         break;
00115       }
00116     }
00117 
00118     if (best_model.size () != 0)
00119     {
00120       if (debug > 0)
00121         std::cerr << "[RANSAC::computeModel] Model found: " << n_best_inliers_count << " inliers." << std::endl;
00122       sac_model_->setBestModel (best_model);
00123       sac_model_->setBestInliers (best_inliers);
00124       return (true);
00125     }
00126     else
00127       if (debug > 0)
00128         std::cerr << "[RANSAC::computeModel] Unable to find a solution!" << std::endl;
00129     return (false);
00130   }
00131 }