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