Point Cloud Library (PCL)
1.3.1
|
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: prosac.hpp 1370 2011-06-19 01:06:01Z jspricke $ 00035 * 00036 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_ 00039 #define PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_ 00040 00041 #include <boost/math/distributions/binomial.hpp> 00042 #include "pcl/sample_consensus/prosac.h" 00043 00045 // Variable naming uses capital letters to make the comparison with the original paper easier 00046 template<typename PointT> bool 00047 pcl::ProgressiveSampleConsensus<PointT>::computeModel (int debug_verbosity_level) 00048 { 00049 // Warn and exit if no threshold was set 00050 if (threshold_ == DBL_MAX) 00051 { 00052 PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No threshold set!\n"); 00053 return (false); 00054 } 00055 00056 // Initialize some PROSAC constants 00057 float T_N = 200000; 00058 unsigned int N = sac_model_->indices_->size (); 00059 unsigned int m = sac_model_->getSampleSize (); 00060 float T_n = T_N; 00061 for (unsigned int i = 0; i < m; ++i) 00062 T_n *= (float)(m - i) / (float)(N - i); 00063 float T_prime_n = 1; 00064 unsigned int I_N_best = 0; 00065 float n = m; 00066 00067 // Define the n_Start coefficients from Section 2.2 00068 float n_star = N; 00069 unsigned int I_n_star = 0; 00070 float epsilon_n_star = (float)I_n_star / n_star; 00071 unsigned int k_n_star = T_N; 00072 00073 // Compute the I_n_star_min of Equation 8 00074 std::vector<unsigned int> I_n_star_min (N); 00075 00076 // Initialize the usual RANSAC parameters 00077 iterations_ = 0; 00078 00079 std::vector<int> inliers; 00080 std::vector<int> selection; 00081 Eigen::VectorXf model_coefficients; 00082 00083 // We will increase the pool so the indices_ vector can only contain m elements at first 00084 std::vector<int> index_pool; 00085 index_pool.reserve (N); 00086 for (unsigned int i = 0; i < n; ++i) 00087 index_pool.push_back (sac_model_->indices_->operator[](i)); 00088 00089 // Iterate 00090 while ((unsigned int)iterations_ < k_n_star) 00091 { 00092 // Choose the samples 00093 00094 // Step 1 00095 // According to Equation 5 in the text text, not the algorithm 00096 if ((iterations_ == T_prime_n) && (n < n_star)) 00097 { 00098 // Increase the pool 00099 ++n; 00100 if (n >= N) 00101 break; 00102 index_pool.push_back (sac_model_->indices_->at(n - 1)); 00103 // Update other variables 00104 float T_n_minus_1 = T_n; 00105 T_n *= (float)(n + 1) / (float)(n + 1 - m); 00106 T_prime_n += ceil(T_n - T_n_minus_1); 00107 } 00108 00109 // Step 2 00110 sac_model_->indices_->swap (index_pool); 00111 selection.clear (); 00112 sac_model_->getSamples (iterations_, selection); 00113 if (T_prime_n < iterations_) 00114 { 00115 selection.pop_back (); 00116 selection.push_back (sac_model_->indices_->at(n - 1)); 00117 } 00118 00119 // Make sure we use the right indices for testing 00120 sac_model_->indices_->swap (index_pool); 00121 00122 if (selection.empty ()) 00123 { 00124 PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No samples could be selected!\n"); 00125 break; 00126 } 00127 00128 // Search for inliers in the point cloud for the current model 00129 if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) 00130 { 00131 ++iterations_; 00132 continue; 00133 } 00134 00135 // Select the inliers that are within threshold_ from the model 00136 inliers.clear (); 00137 sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers); 00138 00139 unsigned int I_N = inliers.size (); 00140 00141 // If we find more inliers than before 00142 if (I_N > I_N_best) 00143 { 00144 I_N_best = I_N; 00145 00146 // Save the current model/inlier/coefficients selection as being the best so far 00147 inliers_ = inliers; 00148 model_ = selection; 00149 model_coefficients_ = model_coefficients; 00150 00151 // We estimate I_n_star for different possible values of n_star by using the inliers 00152 std::sort (inliers.begin (), inliers.end ()); 00153 00154 // Try to find a better n_star 00155 // We minimize k_n_star and therefore maximize epsilon_n_star = I_n_star / n_star 00156 unsigned int possible_n_star_best = N, I_possible_n_star_best = I_N; 00157 float epsilon_possible_n_star_best = (float)I_possible_n_star_best / possible_n_star_best; 00158 00159 // We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier 00160 unsigned int I_possible_n_star = I_N; 00161 for (std::vector<int>::const_reverse_iterator last_inlier = inliers.rbegin (), 00162 inliers_end = inliers.rend (); 00163 last_inlier != inliers_end; 00164 ++last_inlier, --I_possible_n_star) 00165 { 00166 // The best possible_n_star for a given I_possible_n_star is the index of the last inlier 00167 unsigned int possible_n_star = (*last_inlier) + 1; 00168 if (possible_n_star <= m) 00169 break; 00170 00171 // If we find a better epsilon_n_star 00172 float epsilon_possible_n_star = (float)I_possible_n_star / possible_n_star; 00173 // Make sure we have a better epsilon_possible_n_star 00174 if ((epsilon_possible_n_star > epsilon_n_star) && (epsilon_possible_n_star > epsilon_possible_n_star_best)) 00175 { 00176 using namespace boost::math; 00177 // Typo in Equation 7, not (n-m choose i-m) but (n choose i-m) 00178 unsigned int 00179 I_possible_n_star_min = m 00180 + ceil (quantile (complement (binomial_distribution<float>(possible_n_star, 0.1), 0.05))); 00181 // If Equation 9 is not verified, exit 00182 if (I_possible_n_star < I_possible_n_star_min) 00183 break; 00184 00185 possible_n_star_best = possible_n_star; 00186 I_possible_n_star_best = I_possible_n_star; 00187 epsilon_possible_n_star_best = epsilon_possible_n_star; 00188 } 00189 } 00190 00191 // Check if we get a better epsilon 00192 if (epsilon_possible_n_star_best > epsilon_n_star) 00193 { 00194 // update the best value 00195 epsilon_n_star = epsilon_possible_n_star_best; 00196 00197 // Compute the new k_n_star 00198 float bottom_log = 1 - std::pow (epsilon_n_star, static_cast<float>(m)); 00199 if (bottom_log == 0) 00200 k_n_star = 1; 00201 else if (bottom_log == 1) 00202 k_n_star = T_N; 00203 else 00204 k_n_star = (int)ceil (log(0.05) / log (bottom_log)); 00205 // It seems weird to have very few iterations, so do have a few (totally empirical) 00206 k_n_star = (std::max)(k_n_star, 2 * m); 00207 } 00208 } 00209 00210 ++iterations_; 00211 if (debug_verbosity_level > 1) 00212 PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, k_n_star, I_N, I_N_best); 00213 if (iterations_ > max_iterations_) 00214 { 00215 if (debug_verbosity_level > 0) 00216 PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n"); 00217 break; 00218 } 00219 } 00220 00221 if (debug_verbosity_level > 0) 00222 PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), I_N_best); 00223 00224 if (model_.empty ()) 00225 { 00226 inliers_.clear (); 00227 return (false); 00228 } 00229 00230 // Get the set of inliers that correspond to the best model found so far 00231 //sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_); 00232 return (true); 00233 } 00234 00235 #define PCL_INSTANTIATE_ProgressiveSampleConsensus(T) template class PCL_EXPORTS pcl::ProgressiveSampleConsensus<T>; 00236 00237 #endif // PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_