41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ 42 #define PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ 44 #include <pcl/sample_consensus/rransac.h> 47 template <
typename Po
intT>
bool 51 if (threshold_ == std::numeric_limits<double>::max())
53 PCL_ERROR (
"[pcl::RandomizedRandomSampleConsensus::computeModel] No threshold set!\n");
58 int n_best_inliers_count = -INT_MAX;
61 std::vector<int> selection;
62 Eigen::VectorXf model_coefficients;
63 std::set<int> indices_subset;
65 int n_inliers_count = 0;
66 unsigned skipped_count = 0;
68 const unsigned max_skip = max_iterations_ * 10;
71 size_t fraction_nr_points = pcl_lrint (static_cast<double>(sac_model_->getIndices ()->size ()) * fraction_nr_pretest_ / 100.0);
74 while (iterations_ < k && skipped_count < max_skip)
77 sac_model_->getSamples (iterations_, selection);
79 if (selection.empty ())
break;
82 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
91 this->getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset);
92 if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_))
103 n_inliers_count = sac_model_->countWithinDistance (model_coefficients, threshold_);
106 if (n_inliers_count > n_best_inliers_count)
108 n_best_inliers_count = n_inliers_count;
112 model_coefficients_ = model_coefficients;
115 double w =
static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
116 double p_no_outliers = 1 - pow (w, static_cast<double> (selection.size ()));
117 p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);
118 p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers);
119 k = log (1 - probability_) / log (p_no_outliers);
124 if (debug_verbosity_level > 1)
125 PCL_DEBUG (
"[pcl::RandomizedRandomSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, static_cast<int> (ceil (k)), n_inliers_count, n_best_inliers_count);
126 if (iterations_ > max_iterations_)
128 if (debug_verbosity_level > 0)
129 PCL_DEBUG (
"[pcl::RandomizedRandomSampleConsensus::computeModel] RRANSAC reached the maximum number of trials.\n");
134 if (debug_verbosity_level > 0)
135 PCL_DEBUG (
"[pcl::RandomizedRandomSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_best_inliers_count);
144 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
148 #define PCL_INSTANTIATE_RandomizedRandomSampleConsensus(T) template class PCL_EXPORTS pcl::RandomizedRandomSampleConsensus<T>; 150 #endif // PCL_SAMPLE_CONSENSUS_IMPL_RRANSAC_H_ bool computeModel(int debug_verbosity_level=0)
Compute the actual model and find the inliers.