ransac.h
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
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 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #ifndef MATH_UTIL_RANSAC_H_
00031 #define MATH_UTIL_RANSAC_H_
00032 
00033 #include <cmath>
00034 #include <limits>
00035 #include <vector>
00036 
00037 #include <boost/make_shared.hpp>
00038 #include <swri_math_util/random.h>
00039 
00040 namespace swri_math_util
00041 {
00042   template <class Model>
00043   class Ransac
00044   {
00045   public:
00046     typedef typename Model::M ModelType;
00047     typedef typename Model::T DataType;
00048   
00049     explicit Ransac(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}
00050 
00051     ModelType FitModel(
00052       Model& model,
00053       double max_error,
00054       double confidence,
00055       int32_t min_iterations,
00056       int32_t max_iterations,
00057       std::vector<uint32_t>& inliers, 
00058       int32_t& iterations)
00059     {
00060       int32_t breakout = std::numeric_limits<int32_t>::max();
00061       ModelType best_fit;
00062       inliers.clear();
00063       int32_t max_inliers = 0;
00064       
00065       if (!model.ValidData())
00066       {
00067         return best_fit;
00068       }
00069       
00070       if (!rng_)
00071       {
00072         rng_ = boost::make_shared<RandomGenerator>();
00073       }
00074       
00075       std::vector<int32_t> indices;
00076 
00077       ModelType hypothesis;
00078       for (iterations = 0; (iterations < max_iterations && iterations < breakout) || iterations < min_iterations; iterations++)
00079       {
00080         indices.clear();
00081         rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
00082 
00083         // Generate a hypothesis model from the random sample.
00084         // If the sample is not degenerate, calculate the number of inliers.
00085         if (model.GetModel(indices, hypothesis, max_error))
00086         {
00087           int32_t inlier_count = model.GetInlierCount(hypothesis, max_error);
00088           
00089           // Update the best fit hypothesis and inliers if this hypothesis has
00090           // the most inliers so far.
00091           if (inlier_count > max_inliers)
00092           {
00093             max_inliers = inlier_count;
00094             Model::CopyTo(hypothesis, best_fit);
00095             
00096             // Recalculate breakout threshold to see if the fit is good enough.
00097             double ratio = inlier_count / static_cast<double>(model.Size());
00098             double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
00099             if (p_no_outliers == 0)
00100             {
00101               breakout = 0;
00102             }
00103             else if (p_no_outliers < .9999)
00104             {
00105               breakout = std::log(1 - confidence) / std::log(p_no_outliers);
00106             }
00107           }
00108         }
00109       }
00110       
00111       if (max_inliers > 0)
00112       {
00113         model.GetInliers(best_fit, max_error, inliers);
00114       }
00115       return best_fit;
00116     }
00117 
00118   private:
00119     RandomGeneratorPtr rng_;
00120   };
00121 
00122   template <class Model>
00123   class RansacBatch
00124   {
00125   public:
00126     typedef typename Model::M ModelType;
00127     typedef typename Model::T DataType;
00128 
00129     explicit RansacBatch(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}
00130 
00131     ModelType FitModel(
00132       const DataType& data,
00133       double max_error,
00134       double confidence,
00135       int32_t max_iterations,
00136       int32_t batch_size,
00137       std::vector<uint32_t>& inliers,
00138       int32_t& iterations)
00139     {
00140       Model model(data, batch_size);
00141       iterations = 0;
00142       int32_t breakout = std::numeric_limits<int32_t>::max();
00143       ModelType best_fit;
00144       inliers.clear();
00145       int32_t max_inliers = 0;
00146 
00147       if (!model.ValidData())
00148       {
00149         return best_fit;
00150       }
00151 
00152       if (!rng_)
00153       {
00154         rng_ = boost::make_shared<RandomGenerator>();
00155       }
00156 
00157       std::vector<int32_t> indices;
00158 
00159       ModelType hypothesis;
00160       while (iterations < max_iterations && iterations < breakout)
00161       {
00162         int32_t valid = 0;
00163         model.ClearSamples();
00164         while (iterations < max_iterations && iterations < breakout && model.Samples() < batch_size)
00165         {
00166           iterations++;
00167           indices.clear();
00168           rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
00169           model.AddSample(indices, max_error);
00170         }
00171 
00172         if (model.Samples() > 0)
00173         {
00174           int32_t inlier_count = model.ProcessSamples(hypothesis, max_error);
00175           if (inlier_count > 0 && inlier_count > max_inliers)
00176           {
00177             max_inliers = inlier_count;
00178             Model::CopyTo(hypothesis, best_fit);
00179 
00180             // Recalculate breakout threshold to see if the fit is good enough.
00181             double ratio = inlier_count / static_cast<double>(model.Size());
00182             double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
00183             if (p_no_outliers == 0)
00184             {
00185               breakout = 0;
00186             }
00187             else if (p_no_outliers < .9999)
00188             {
00189               breakout = std::log(1 - confidence) / std::log(p_no_outliers);
00190             }
00191           }
00192         }
00193       }
00194 
00195       if (max_inliers > 0)
00196       {
00197         model.GetInliers(best_fit, max_error, inliers);
00198       }
00199       return best_fit;
00200     }
00201 
00202   private:
00203     RandomGeneratorPtr rng_;
00204   };
00205 }
00206 
00207 #endif  // MATH_UTIL_RANSAC_H_


swri_math_util
Author(s): Marc Alban
autogenerated on Thu Jun 6 2019 20:34:41