Point Cloud Library (PCL)  1.12.1-dev
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 //////////////////////////////////////////////////////////////////////////
49 template <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 //////////////////////////////////////////////////////////////////////////
211 template <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 //////////////////////////////////////////////////////////////////////////
236 template <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 //////////////////////////////////////////////////////////////////////////
260 template <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
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
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