/** \brief Compute the actual model and find the inliers * \param debug enable/disable on-screen debug information */ bool RMSAC::computeModel (int debug) { iterations_ = 0; double d_best_penalty = DBL_MAX; double k = 1.0; std::vector<int> best_model; std::vector<int> best_inliers, inliers; std::vector<int> selection; std::vector<double> distances; int n_inliers_count = 0; // Number of samples to try randomly in percents int fraction_nr_points = lrint (sac_model_->getIndices ()->size () * fraction_nr_pretest_ / 100.0); // Iterate while (iterations_ < k) { // Get X samples which satisfy the model criteria sac_model_->getSamples (iterations_, selection); if (selection.size () == 0) break; // Search for inliers in the point cloud for the current plane model M sac_model_->computeModelCoefficients (selection); // RMSAC addon: verify a random fraction of the data // Get X random samples which satisfy the model criteria std::set<int> fraction_idx = getRandomSamples (*sac_model_->getCloud (), *sac_model_->getIndices (), fraction_nr_points); if (!sac_model_->doSamplesVerifyModel (fraction_idx, threshold_)) { // Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented if (k != 1.0) { iterations_ += 1; continue; } } double d_cur_penalty = 0; // d_cur_penalty = sum (min (dist, threshold)) // Iterate through the 3d points and calculate the distances from them to the model sac_model_->getDistancesToModel (sac_model_->getModelCoefficients (), distances); if (distances.size () == 0 && k != 1.0) { iterations_ += 1; continue; } for (unsigned int i = 0; i < sac_model_->getIndices ()->size (); i++) d_cur_penalty += std::min ((double)distances[i], threshold_); // Better match ? if (d_cur_penalty < d_best_penalty) { d_best_penalty = d_cur_penalty; best_model = selection; // Save the inliers best_inliers.resize (sac_model_->getIndices ()->size ()); n_inliers_count = 0; for (unsigned int i = 0; i < sac_model_->getIndices ()->size (); i++) { if (distances[i] <= threshold_) { best_inliers[n_inliers_count] = sac_model_->getIndices ()->at (i); n_inliers_count++; } } best_inliers.resize (n_inliers_count); // Compute the k parameter (k=log(z)/log(1-w^n)) double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ()); double p_no_outliers = 1 - pow (w, (double)selection.size ()); p_no_outliers = std::max (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf p_no_outliers = std::min (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0 k = log (1 - probability_) / log (p_no_outliers); } iterations_ += 1; if (debug > 1) std::cerr << "[RMSAC::computeModel] Trial " << iterations_ << " out of " << ceil (k) << ": best number of inliers so far is " << best_inliers.size () << "." << std::endl; if (iterations_ > max_iterations_) { if (debug > 0) std::cerr << "[RMSAC::computeModel] MSAC reached the maximum number of trials." << std::endl; break; } } if (best_model.size () != 0) { if (debug > 0) std::cerr << "[RMSAC::computeModel] Model found: " << best_inliers.size () << " inliers." << std::endl; sac_model_->setBestModel (best_model); sac_model_->setBestInliers (best_inliers); return (true); } else if (debug > 0) std::cerr << "[RMSAC::computeModel] Unable to find a solution!" << std::endl; return (false); }
template <typename PointT> bool pcl::RandomizedMEstimatorSampleConsensus<PointT>::computeModel (int debug_verbosity_level) { // Warn and exit if no threshold was set if (threshold_ == DBL_MAX) { ROS_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] No threshold set!"); return (false); } iterations_ = 0; double d_best_penalty = DBL_MAX; double k = 1.0; std::vector<int> best_model; std::vector<int> selection; Eigen::VectorXf model_coefficients; std::vector<double> distances; std::set<int> indices_subset; int n_inliers_count = 0; // Number of samples to try randomly size_t fraction_nr_points = lrint (sac_model_->getIndices ()->size () * fraction_nr_pretest_ / 100.0); // Iterate while (iterations_ < k) { // Get X samples which satisfy the model criteria sac_model_->getSamples (iterations_, selection); if (selection.empty ()) break; // Search for inliers in the point cloud for the current plane model M if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) { //iterations_++; continue; } // RMSAC addon: verify a random fraction of the data // Get X random samples which satisfy the model criterion getRandomSamples (sac_model_->getIndices (), fraction_nr_points, indices_subset); if (!sac_model_->doSamplesVerifyModel (indices_subset, model_coefficients, threshold_)) { // Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented if (k != 1.0) { ++iterations_; continue; } } double d_cur_penalty = 0; // Iterate through the 3d points and calculate the distances from them to the model sac_model_->getDistancesToModel (model_coefficients, distances); if (distances.empty () && k > 1.0) continue; for (size_t i = 0; i < distances.size (); ++i) d_cur_penalty += (std::min) (distances[i], threshold_); // Better match ? if (d_cur_penalty < d_best_penalty) { d_best_penalty = d_cur_penalty; // Save the current model/coefficients selection as being the best so far model_ = selection; model_coefficients_ = model_coefficients; n_inliers_count = 0; // Need to compute the number of inliers for this model to adapt k for (size_t i = 0; i < distances.size (); ++i) if (distances[i] <= threshold_) n_inliers_count++; // Compute the k parameter (k=log(z)/log(1-w^n)) double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ()); double p_no_outliers = 1 - pow (w, (double)selection.size ()); p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0. k = log (1 - probability_) / log (p_no_outliers); } ++iterations_; if (debug_verbosity_level > 1) ROS_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.", iterations_, (int)ceil (k), d_best_penalty); if (iterations_ > max_iterations_) { if (debug_verbosity_level > 0) ROS_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] MSAC reached the maximum number of trials."); break; } } if (model_.empty ()) { if (debug_verbosity_level > 0) ROS_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Unable to find a solution!"); return (false); } // Iterate through the 3d points and calculate the distances from them to the model again sac_model_->getDistancesToModel (model_coefficients_, distances); std::vector<int> &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { ROS_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).", distances.size (), indices.size ()); return (false); } inliers_.resize (distances.size ()); // Get the inliers for the best model found n_inliers_count = 0; for (size_t i = 0; i < distances.size (); ++i) if (distances[i] <= threshold_) inliers_[n_inliers_count++] = indices[i]; // Resize the inliers vector inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) ROS_DEBUG ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Model: %zu size, %d inliers.", model_.size (), n_inliers_count); return (true); }
/** \brief Compute the actual model and find the inliers * \param debug enable/disable on-screen debug information */ bool RRANSAC::computeModel (int debug) { iterations_ = 0; int n_best_inliers_count = -INT_MAX; double k = 1.0; std::vector<int> best_model; std::vector<int> best_inliers, inliers; std::vector<int> selection; int n_inliers_count = 0; // Number of samples to try randomly in percents int fraction_nr_points = lrint (sac_model_->getIndices ()->size () * fraction_nr_pretest_ / 100.0); // Iterate while (iterations_ < k) { // Get X samples which satisfy the model criteria sac_model_->getSamples (iterations_, selection); if (selection.size () == 0) break; // Search for inliers in the point cloud for the current plane model M sac_model_->computeModelCoefficients (selection); // RRANSAC addon: verify a random fraction of the data // Get X random samples which satisfy the model criteria std::set<int> fraction_idx = getRandomSamples (*sac_model_->getCloud (), *sac_model_->getIndices (), fraction_nr_points); if (!sac_model_->doSamplesVerifyModel (fraction_idx, threshold_)) { // Unfortunately we cannot "continue" after the first iteration, because k might not be set, while iterations gets incremented if (k != 1.0) { iterations_ += 1; continue; } } sac_model_->selectWithinDistance (sac_model_->getModelCoefficients (), threshold_, inliers); n_inliers_count = inliers.size (); // Better match ? if (n_inliers_count > n_best_inliers_count) { n_best_inliers_count = n_inliers_count; best_inliers = inliers; //inliers.clear (); best_model = selection; // Compute the k parameter (k=log(z)/log(1-w^n)) double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ()); double p_no_outliers = 1 - pow (w, (double)selection.size ()); p_no_outliers = std::max (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf p_no_outliers = std::min (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0. k = log (1 - probability_) / log (p_no_outliers); } iterations_ += 1; if (debug > 1) std::cerr << "[RRANSAC::computeModel] Trial " << iterations_ << " out of " << ceil (k) << ": " << n_inliers_count << " inliers (best is: " << n_best_inliers_count << " so far)." << std::endl; if (iterations_ > max_iterations_) { if (debug > 0) std::cerr << "[RRANSAC::computeModel] RANSAC reached the maximum number of trials." << std::endl; break; } } if (best_model.size () != 0) { if (debug > 0) std::cerr << "[RRANSAC::computeModel] Model found: " << n_best_inliers_count << " inliers." << std::endl; sac_model_->setBestModel (best_model); sac_model_->setBestInliers (best_inliers); return (true); } else if (debug > 0) std::cerr << "[RRANSAC::computeModel] Unable to find a solution!" << std::endl; return (false); }