41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ 42 #define PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ 44 #include <pcl/sample_consensus/ransac.h> 47 template <
typename Po
intT>
bool 51 if (threshold_ == std::numeric_limits<double>::max())
53 PCL_ERROR (
"[pcl::RandomSampleConsensus::computeModel] No threshold set!\n");
58 int n_best_inliers_count = -INT_MAX;
61 std::vector<int> selection;
62 Eigen::VectorXf model_coefficients;
64 double log_probability = log (1.0 - probability_);
65 double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
67 int n_inliers_count = 0;
68 unsigned skipped_count = 0;
70 const unsigned max_skip = max_iterations_ * 10;
73 while (iterations_ < k && skipped_count < max_skip)
76 sac_model_->getSamples (iterations_, selection);
78 if (selection.empty ())
80 PCL_ERROR (
"[pcl::RandomSampleConsensus::computeModel] No samples could be selected!\n");
85 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
97 n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
100 if (n_inliers_count > n_best_inliers_count)
102 n_best_inliers_count = n_inliers_count;
106 model_coefficients_ = model_coefficients;
109 double w = static_cast<double> (n_best_inliers_count) * one_over_indices;
110 double p_no_outliers = 1.0 - pow (w, static_cast<double> (selection.size ()));
111 p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);
112 p_no_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_no_outliers);
113 k = log_probability / log (p_no_outliers);
117 PCL_DEBUG (
"[pcl::RandomSampleConsensus::computeModel] Trial %d out of %f: %d inliers (best is: %d so far).\n", iterations_, k, n_inliers_count, n_best_inliers_count);
118 if (iterations_ > max_iterations_)
120 PCL_DEBUG (
"[pcl::RandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
125 PCL_DEBUG (
"[pcl::RandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_best_inliers_count);
134 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
138 #define PCL_INSTANTIATE_RandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomSampleConsensus<T>; 140 #endif // PCL_SAMPLE_CONSENSUS_IMPL_RANSAC_H_ bool computeModel(int debug_verbosity_level=0)
Compute the actual model and find the inliers.