Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, 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_H_ 00042 #define PCL_SAMPLE_CONSENSUS_H_ 00043 00044 #include <pcl/sample_consensus/boost.h> 00045 #include <pcl/sample_consensus/sac_model.h> 00046 #include <ctime> 00047 #include <set> 00048 00049 namespace pcl 00050 { 00051 /** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class. 00052 * \author Radu Bogdan Rusu 00053 * \ingroup sample_consensus 00054 */ 00055 template <typename T> 00056 class SampleConsensus 00057 { 00058 typedef typename SampleConsensusModel<T>::Ptr SampleConsensusModelPtr; 00059 00060 private: 00061 /** \brief Constructor for base SAC. */ 00062 SampleConsensus () {}; 00063 00064 public: 00065 typedef boost::shared_ptr<SampleConsensus> Ptr; 00066 typedef boost::shared_ptr<const SampleConsensus> ConstPtr; 00067 00068 /** \brief Constructor for base SAC. 00069 * \param[in] model a Sample Consensus model 00070 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00071 */ 00072 SampleConsensus (const SampleConsensusModelPtr &model, bool random = false) 00073 : sac_model_ (model) 00074 , model_ () 00075 , inliers_ () 00076 , model_coefficients_ () 00077 , probability_ (0.99) 00078 , iterations_ (0) 00079 , threshold_ (std::numeric_limits<double>::max ()) 00080 , max_iterations_ (1000) 00081 , rng_alg_ () 00082 , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_)) 00083 { 00084 // Create a random number generator object 00085 if (random) 00086 rng_->base ().seed (static_cast<unsigned> (std::time (0))); 00087 else 00088 rng_->base ().seed (12345u); 00089 }; 00090 00091 /** \brief Constructor for base SAC. 00092 * \param[in] model a Sample Consensus model 00093 * \param[in] threshold distance to model threshol 00094 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00095 */ 00096 SampleConsensus (const SampleConsensusModelPtr &model, 00097 double threshold, 00098 bool random = false) 00099 : sac_model_ (model) 00100 , model_ () 00101 , inliers_ () 00102 , model_coefficients_ () 00103 , probability_ (0.99) 00104 , iterations_ (0) 00105 , threshold_ (threshold) 00106 , max_iterations_ (1000) 00107 , rng_alg_ () 00108 , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_)) 00109 { 00110 // Create a random number generator object 00111 if (random) 00112 rng_->base ().seed (static_cast<unsigned> (std::time (0))); 00113 else 00114 rng_->base ().seed (12345u); 00115 }; 00116 00117 /** \brief Set the Sample Consensus model to use. 00118 * \param[in] model a Sample Consensus model 00119 */ 00120 void 00121 setSampleConsensusModel (const SampleConsensusModelPtr &model) 00122 { 00123 sac_model_ = model; 00124 } 00125 00126 /** \brief Get the Sample Consensus model used. */ 00127 SampleConsensusModelPtr 00128 getSampleConsensusModel () const 00129 { 00130 return (sac_model_); 00131 } 00132 00133 /** \brief Destructor for base SAC. */ 00134 virtual ~SampleConsensus () {}; 00135 00136 /** \brief Set the distance to model threshold. 00137 * \param[in] threshold distance to model threshold 00138 */ 00139 inline void 00140 setDistanceThreshold (double threshold) { threshold_ = threshold; } 00141 00142 /** \brief Get the distance to model threshold, as set by the user. */ 00143 inline double 00144 getDistanceThreshold () { return (threshold_); } 00145 00146 /** \brief Set the maximum number of iterations. 00147 * \param[in] max_iterations maximum number of iterations 00148 */ 00149 inline void 00150 setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; } 00151 00152 /** \brief Get the maximum number of iterations, as set by the user. */ 00153 inline int 00154 getMaxIterations () { return (max_iterations_); } 00155 00156 /** \brief Set the desired probability of choosing at least one sample free from outliers. 00157 * \param[in] probability the desired probability of choosing at least one sample free from outliers 00158 * \note internally, the probability is set to 99% (0.99) by default. 00159 */ 00160 inline void 00161 setProbability (double probability) { probability_ = probability; } 00162 00163 /** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */ 00164 inline double 00165 getProbability () { return (probability_); } 00166 00167 /** \brief Compute the actual model. Pure virtual. */ 00168 virtual bool 00169 computeModel (int debug_verbosity_level = 0) = 0; 00170 00171 /** \brief Refine the model found. 00172 * This loops over the model coefficients and optimizes them together 00173 * with the set of inliers, until the change in the set of inliers is 00174 * minimal. 00175 * \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance) 00176 * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing 00177 */ 00178 virtual bool 00179 refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000) 00180 { 00181 if (!sac_model_) 00182 { 00183 PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n"); 00184 return (false); 00185 } 00186 00187 double inlier_distance_threshold_sqr = threshold_ * threshold_, 00188 error_threshold = threshold_; 00189 double sigma_sqr = sigma * sigma; 00190 unsigned int refine_iterations = 0; 00191 bool inlier_changed = false, oscillating = false; 00192 std::vector<int> new_inliers, prev_inliers = inliers_; 00193 std::vector<size_t> inliers_sizes; 00194 Eigen::VectorXf new_model_coefficients = model_coefficients_; 00195 do 00196 { 00197 // Optimize the model coefficients 00198 sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients); 00199 inliers_sizes.push_back (prev_inliers.size ()); 00200 00201 // Select the new inliers based on the optimized coefficients and new threshold 00202 sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers); 00203 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold); 00204 00205 if (new_inliers.empty ()) 00206 { 00207 refine_iterations++; 00208 if (refine_iterations >= max_iterations) 00209 break; 00210 continue; 00211 //return (false); 00212 } 00213 00214 // Estimate the variance and the new threshold 00215 double variance = sac_model_->computeVariance (); 00216 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance)); 00217 00218 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations); 00219 inlier_changed = false; 00220 std::swap (prev_inliers, new_inliers); 00221 // If the number of inliers changed, then we are still optimizing 00222 if (new_inliers.size () != prev_inliers.size ()) 00223 { 00224 // Check if the number of inliers is oscillating in between two values 00225 if (inliers_sizes.size () >= 4) 00226 { 00227 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] && 00228 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4]) 00229 { 00230 oscillating = true; 00231 break; 00232 } 00233 } 00234 inlier_changed = true; 00235 continue; 00236 } 00237 00238 // Check the values of the inlier set 00239 for (size_t i = 0; i < prev_inliers.size (); ++i) 00240 { 00241 // If the value of the inliers changed, then we are still optimizing 00242 if (prev_inliers[i] != new_inliers[i]) 00243 { 00244 inlier_changed = true; 00245 break; 00246 } 00247 } 00248 } 00249 while (inlier_changed && ++refine_iterations < max_iterations); 00250 00251 // If the new set of inliers is empty, we didn't do a good job refining 00252 if (new_inliers.empty ()) 00253 { 00254 PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n"); 00255 return (false); 00256 } 00257 00258 if (oscillating) 00259 { 00260 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n"); 00261 return (true); 00262 } 00263 00264 // If no inliers have been changed anymore, then the refinement was successful 00265 if (!inlier_changed) 00266 { 00267 std::swap (inliers_, new_inliers); 00268 model_coefficients_ = new_model_coefficients; 00269 return (true); 00270 } 00271 return (false); 00272 } 00273 00274 /** \brief Get a set of randomly selected indices. 00275 * \param[in] indices the input indices vector 00276 * \param[in] nr_samples the desired number of point indices to randomly select 00277 * \param[out] indices_subset the resultant output set of randomly selected indices 00278 */ 00279 inline void 00280 getRandomSamples (const boost::shared_ptr <std::vector<int> > &indices, 00281 size_t nr_samples, 00282 std::set<int> &indices_subset) 00283 { 00284 indices_subset.clear (); 00285 while (indices_subset.size () < nr_samples) 00286 //indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]); 00287 indices_subset.insert ((*indices)[static_cast<int> (static_cast<double>(indices->size ()) * rnd ())]); 00288 } 00289 00290 /** \brief Return the best model found so far. 00291 * \param[out] model the resultant model 00292 */ 00293 inline void 00294 getModel (std::vector<int> &model) { model = model_; } 00295 00296 /** \brief Return the best set of inliers found so far for this model. 00297 * \param[out] inliers the resultant set of inliers 00298 */ 00299 inline void 00300 getInliers (std::vector<int> &inliers) { inliers = inliers_; } 00301 00302 /** \brief Return the model coefficients of the best model found so far. 00303 * \param[out] model_coefficients the resultant model coefficients 00304 */ 00305 inline void 00306 getModelCoefficients (Eigen::VectorXf &model_coefficients) { model_coefficients = model_coefficients_; } 00307 00308 protected: 00309 /** \brief The underlying data model used (i.e. what is it that we attempt to search for). */ 00310 SampleConsensusModelPtr sac_model_; 00311 00312 /** \brief The model found after the last computeModel () as point cloud indices. */ 00313 std::vector<int> model_; 00314 00315 /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */ 00316 std::vector<int> inliers_; 00317 00318 /** \brief The coefficients of our model computed directly from the model found. */ 00319 Eigen::VectorXf model_coefficients_; 00320 00321 /** \brief Desired probability of choosing at least one sample free from outliers. */ 00322 double probability_; 00323 00324 /** \brief Total number of internal loop iterations that we've done so far. */ 00325 int iterations_; 00326 00327 /** \brief Distance to model threshold. */ 00328 double threshold_; 00329 00330 /** \brief Maximum number of iterations before giving up. */ 00331 int max_iterations_; 00332 00333 /** \brief Boost-based random number generator algorithm. */ 00334 boost::mt19937 rng_alg_; 00335 00336 /** \brief Boost-based random number generator distribution. */ 00337 boost::shared_ptr<boost::uniform_01<boost::mt19937> > rng_; 00338 00339 /** \brief Boost-based random number generator. */ 00340 inline double 00341 rnd () 00342 { 00343 return ((*rng_) ()); 00344 } 00345 }; 00346 } 00347 00348 #endif //#ifndef PCL_SAMPLE_CONSENSUS_H_