Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
mlesac.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_
43
44#include <limits>
45#include <pcl/sample_consensus/mlesac.h>
46#include <pcl/common/common.h> // for computeMedian
47
48//////////////////////////////////////////////////////////////////////////
49template <typename PointT> bool
51{
52 // Warn and exit if no threshold was set
53 if (threshold_ == std::numeric_limits<double>::max())
54 {
55 PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
56 return (false);
57 }
58
59 iterations_ = 0;
60 double d_best_penalty = std::numeric_limits<double>::max();
61 double k = 1.0;
62
63 const double log_probability = std::log (1.0 - probability_);
64 const double one_over_indices = 1.0 / static_cast<double> (sac_model_->getIndices ()->size ());
65
66 Indices selection;
67 Eigen::VectorXf model_coefficients (sac_model_->getModelSize ());
68 std::vector<double> distances;
69
70 // Compute sigma - remember to set threshold_ correctly !
71 sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
72 const double dist_scaling_factor = -1.0 / (2.0 * sigma_ * sigma_); // Precompute since this does not change
73 const double normalization_factor = 1.0 / (sqrt (2 * M_PI) * sigma_);
74 if (debug_verbosity_level > 1)
75 PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);
76
77 // Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2))
78 Eigen::Vector4f min_pt, max_pt;
79 getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
80 max_pt -= min_pt;
81 double v = sqrt (max_pt.dot (max_pt));
82
83 int n_inliers_count = 0;
84 std::size_t indices_size;
85 unsigned skipped_count = 0;
86 // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
87 const unsigned max_skip = max_iterations_ * 10;
88
89 // Iterate
90 while (iterations_ < k && skipped_count < max_skip)
91 {
92 // Get X samples which satisfy the model criteria
93 sac_model_->getSamples (iterations_, selection);
94
95 if (selection.empty ()) break;
96
97 // Search for inliers in the point cloud for the current plane model M
98 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
99 {
100 //iterations_++;
101 ++ skipped_count;
102 continue;
103 }
104
105 // Iterate through the 3d points and calculate the distances from them to the model
106 sac_model_->getDistancesToModel (model_coefficients, distances);
107
108 if (distances.empty ())
109 {
110 //iterations_++;
111 ++skipped_count;
112 continue;
113 }
114
115 // Use Expectation-Maximization to find out the right value for d_cur_penalty
116 // ---[ Initial estimate for the gamma mixing parameter = 1/2
117 double gamma = 0.5;
118 double p_outlier_prob = 0;
119
120 indices_size = sac_model_->getIndices ()->size ();
121 std::vector<double> p_inlier_prob (indices_size);
122 for (int j = 0; j < iterations_EM_; ++j)
123 {
124 const double weighted_normalization_factor = gamma * normalization_factor;
125 // Likelihood of a datum given that it is an inlier
126 for (std::size_t i = 0; i < indices_size; ++i)
127 p_inlier_prob[i] = weighted_normalization_factor * std::exp ( dist_scaling_factor * distances[i] * distances[i] );
128
129 // Likelihood of a datum given that it is an outlier
130 p_outlier_prob = (1 - gamma) / v;
131
132 gamma = 0;
133 for (std::size_t i = 0; i < indices_size; ++i)
134 gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob);
135 gamma /= static_cast<double>(sac_model_->getIndices ()->size ());
136 }
137
138 // Find the std::log likelihood of the model -L = -sum [std::log (pInlierProb + pOutlierProb)]
139 double d_cur_penalty = 0;
140 for (std::size_t i = 0; i < indices_size; ++i)
141 d_cur_penalty += std::log (p_inlier_prob[i] + p_outlier_prob);
142 d_cur_penalty = - d_cur_penalty;
143
144 // Better match ?
145 if (d_cur_penalty < d_best_penalty)
146 {
147 d_best_penalty = d_cur_penalty;
148
149 // Save the current model/coefficients selection as being the best so far
150 model_ = selection;
151 model_coefficients_ = model_coefficients;
152
153 n_inliers_count = 0;
154 // Need to compute the number of inliers for this model to adapt k
155 for (const double &distance : distances)
156 if (distance <= 2 * sigma_)
157 n_inliers_count++;
158
159 // Compute the k parameter (k=std::log(z)/std::log(1-w^n))
160 const double w = static_cast<double> (n_inliers_count) * one_over_indices;
161 double p_outliers = 1.0 - std::pow (w, static_cast<double> (selection.size ())); // Probability that selection is contaminated by at least one outlier
162 p_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_outliers); // Avoid division by -Inf
163 p_outliers = (std::min) (1.0 - std::numeric_limits<double>::epsilon (), p_outliers); // Avoid division by 0.
164 k = log_probability / std::log (p_outliers);
165 }
166
167 ++iterations_;
168 if (debug_verbosity_level > 1)
169 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);
170 if (iterations_ > max_iterations_)
171 {
172 if (debug_verbosity_level > 0)
173 PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
174 break;
175 }
176 }
177
178 if (model_.empty ())
179 {
180 if (debug_verbosity_level > 0)
181 PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
182 return (false);
183 }
184
185 // Iterate through the 3d points and calculate the distances from them to the model again
186 sac_model_->getDistancesToModel (model_coefficients_, distances);
187 Indices &indices = *sac_model_->getIndices ();
188 if (distances.size () != indices.size ())
189 {
190 PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
191 return (false);
192 }
193
194 inliers_.resize (distances.size ());
195 // Get the inliers for the best model found
196 n_inliers_count = 0;
197 for (std::size_t i = 0; i < distances.size (); ++i)
198 if (distances[i] <= 2 * sigma_)
199 inliers_[n_inliers_count++] = indices[i];
200
201 // Resize the inliers vector
202 inliers_.resize (n_inliers_count);
203
204 if (debug_verbosity_level > 0)
205 PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
206
207 return (true);
208}
209
210//////////////////////////////////////////////////////////////////////////
211template <typename PointT> double
213 const PointCloudConstPtr &cloud,
214 const IndicesPtr &indices,
215 double sigma) const
216{
217 std::vector<double> distances (indices->size ());
218
219 Eigen::Vector4f median;
220 // median (dist (x - median (x)))
221 computeMedian (cloud, indices, median);
222
223 for (std::size_t i = 0; i < indices->size (); ++i)
224 {
225 pcl::Vector4fMapConst pt = (*cloud)[(*indices)[i]].getVector4fMap ();
226 Eigen::Vector4f ptdiff = pt - median;
227 ptdiff[3] = 0;
228 distances[i] = ptdiff.dot (ptdiff);
229 }
230
231 const double result = pcl::computeMedian (distances.begin (), distances.end (), static_cast<double(*)(double)>(std::sqrt));
232 return (sigma * result);
233}
234
235//////////////////////////////////////////////////////////////////////////
236template <typename PointT> void
238 const PointCloudConstPtr &cloud,
239 const IndicesPtr &indices,
240 Eigen::Vector4f &min_p,
241 Eigen::Vector4f &max_p) const
242{
243 min_p.setConstant (std::numeric_limits<float>::max());
244 max_p.setConstant (std::numeric_limits<float>::lowest());
245 min_p[3] = max_p[3] = 0;
246
247 for (std::size_t i = 0; i < indices->size (); ++i)
248 {
249 if ((*cloud)[(*indices)[i]].x < min_p[0]) min_p[0] = (*cloud)[(*indices)[i]].x;
250 if ((*cloud)[(*indices)[i]].y < min_p[1]) min_p[1] = (*cloud)[(*indices)[i]].y;
251 if ((*cloud)[(*indices)[i]].z < min_p[2]) min_p[2] = (*cloud)[(*indices)[i]].z;
252
253 if ((*cloud)[(*indices)[i]].x > max_p[0]) max_p[0] = (*cloud)[(*indices)[i]].x;
254 if ((*cloud)[(*indices)[i]].y > max_p[1]) max_p[1] = (*cloud)[(*indices)[i]].y;
255 if ((*cloud)[(*indices)[i]].z > max_p[2]) max_p[2] = (*cloud)[(*indices)[i]].z;
256 }
257}
258
259//////////////////////////////////////////////////////////////////////////
260template <typename PointT> void
262 const PointCloudConstPtr &cloud,
263 const IndicesPtr &indices,
264 Eigen::Vector4f &median) const
265{
266 // Copy the values to vectors for faster sorting
267 std::vector<float> x (indices->size ());
268 std::vector<float> y (indices->size ());
269 std::vector<float> z (indices->size ());
270 for (std::size_t i = 0; i < indices->size (); ++i)
271 {
272 x[i] = (*cloud)[(*indices)[i]].x;
273 y[i] = (*cloud)[(*indices)[i]].y;
274 z[i] = (*cloud)[(*indices)[i]].z;
275 }
276
277 median[0] = pcl::computeMedian (x.begin(), x.end());
278 median[1] = pcl::computeMedian (y.begin(), y.end());
279 median[2] = pcl::computeMedian (z.begin(), z.end());
280 median[3] = 0;
281}
282
283#define PCL_INSTANTIATE_MaximumLikelihoodSampleConsensus(T) template class PCL_EXPORTS pcl::MaximumLikelihoodSampleConsensus<T>;
284
285#endif // PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_
286
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...
Definition mlesac.hpp:261
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.
Definition mlesac.hpp:237
bool computeModel(int debug_verbosity_level=0) override
Compute the actual model and find the inliers.
Definition mlesac.hpp:50
double computeMedianAbsoluteDeviation(const PointCloudConstPtr &cloud, const IndicesPtr &indices, double sigma) const
Compute the median absolute deviation:
Definition mlesac.hpp:212
Define standard C methods and C++ classes that are common to all methods.
auto computeMedian(IteratorT begin, IteratorT end, Functor f) noexcept -> std::result_of_t< Functor(decltype(*begin))>
Compute the median of a list of values (fast).
Definition common.h:285
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
Definition common.hpp:400
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
#define M_PI
Definition pcl_macros.h:201