53 if (threshold_ == std::numeric_limits<double>::max())
55 PCL_ERROR (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
65 std::vector<double> distances;
68 sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
72 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);
84 const unsigned max_skip = max_iterations_ * 10;
90 sac_model_->getSamples (iterations_,
selection);
105 if (distances.empty ())
119 for (
int j = 0; j < iterations_EM_; ++j)
132 gamma /=
static_cast<double>(sac_model_->getIndices ()->size ());
152 for (
const double &distance : distances)
153 if (distance <= 2 * sigma_)
157 double w =
static_cast<double> (
n_inliers_count) /
static_cast<double> (sac_model_->getIndices ()->size ());
166 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_,
static_cast<int> (std::ceil (k)),
d_best_penalty);
167 if (iterations_ > max_iterations_)
170 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
178 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
183 sac_model_->getDistancesToModel (model_coefficients_, distances);
184 Indices &indices = *sac_model_->getIndices ();
185 if (distances.size () != indices.
size ())
187 PCL_ERROR (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.
size ());
191 inliers_.resize (distances.size ());
194 for (std::size_t i = 0; i < distances.size (); ++i)
195 if (distances[i] <= 2 * sigma_)
202 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (),
n_inliers_count);
235 const PointCloudConstPtr &cloud,
237 Eigen::Vector4f &
min_p,
238 Eigen::Vector4f &
max_p)
const
244 for (std::size_t i = 0; i < indices->size (); ++i)
246 if ((*cloud)[(*indices)[i]].x <
min_p[0])
min_p[0] = (*cloud)[(*indices)[i]].x;
247 if ((*cloud)[(*indices)[i]].y <
min_p[1])
min_p[1] = (*cloud)[(*indices)[i]].y;
248 if ((*cloud)[(*indices)[i]].z <
min_p[2])
min_p[2] = (*cloud)[(*indices)[i]].z;
250 if ((*cloud)[(*indices)[i]].x >
max_p[0])
max_p[0] = (*cloud)[(*indices)[i]].x;
251 if ((*cloud)[(*indices)[i]].y >
max_p[1])
max_p[1] = (*cloud)[(*indices)[i]].y;
252 if ((*cloud)[(*indices)[i]].z >
max_p[2])
max_p[2] = (*cloud)[(*indices)[i]].z;
259 const PointCloudConstPtr &cloud,
261 Eigen::Vector4f &
median)
const
264 std::vector<float> x (indices->size ());
265 std::vector<float> y (indices->size ());
266 std::vector<float> z (indices->size ());
267 for (std::size_t i = 0; i < indices->size (); ++i)
269 x[i] = (*cloud)[(*indices)[i]].x;
270 y[i] = (*cloud)[(*indices)[i]].y;
271 z[i] = (*cloud)[(*indices)[i]].z;
void computeMedian(const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &median) const
Compute the median value of a 3D point cloud using a given set point indices and return it as a Point...
void getMinMax(const PointCloudConstPtr &cloud, const IndicesPtr &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p) const
Determine the minimum and maximum 3D bounding box coordinates for a given set of points.
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).
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.