52 if (threshold_ == std::numeric_limits<double>::max())
54 PCL_ERROR (
"[pcl::LeastMedianSquares::computeModel] No threshold set!\n");
63 std::vector<double> distances;
67 const unsigned max_skip = max_iterations_ * 10;
73 sac_model_->getSamples (iterations_,
selection);
77 PCL_ERROR (
"[pcl::LeastMedianSquares::computeModel] No samples could be selected!\n");
86 PCL_DEBUG (
"[pcl::LeastMedianSquares::computeModel] The function computeModelCoefficients failed, so continue with next iteration.\n");
97 if (distances.empty ())
104 const auto new_end = (sac_model_->getInputCloud()->is_dense ? distances.end() : std::partition (distances.begin(), distances.end(), [](
double d){return !std::isnan (d);}));
108 PCL_DEBUG (
"[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n",
nr_valid_dists);
130 PCL_DEBUG (
"[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_,
d_best_penalty);
138 PCL_DEBUG (
"[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n");
150 sac_model_->getDistancesToModel (model_coefficients_, distances);
152 if (distances.empty ())
154 PCL_ERROR (
"[pcl::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n");
158 Indices &indices = *sac_model_->getIndices ();
160 if (distances.size () != indices.
size ())
162 PCL_ERROR (
"[pcl::LeastMedianSquares::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.
size ());
166 inliers_.resize (distances.size ());
169 for (std::size_t i = 0; i < distances.size (); ++i)
171 if (distances[i] <= threshold_)
182 PCL_DEBUG (
"[pcl::LeastMedianSquares::computeModel] Model: %lu size, %lu inliers.\n", model_.size (),
n_inliers_count);
auto computeMedian(IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::result_of< Functor(decltype(*begin))>::type
Compute the median of a list of values (fast).