Point Cloud Library (PCL)  1.11.1
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 <pcl/sample_consensus/mlesac.h>
45 #include <pcl/point_types.h>
46 
47 //////////////////////////////////////////////////////////////////////////
48 template <typename PointT> bool
50 {
51  // Warn and exit if no threshold was set
52  if (threshold_ == std::numeric_limits<double>::max())
53  {
54  PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
55  return (false);
56  }
57 
58  iterations_ = 0;
59  double d_best_penalty = std::numeric_limits<double>::max();
60  double k = 1.0;
61 
62  Indices selection;
63  Eigen::VectorXf model_coefficients;
64  std::vector<double> distances;
65 
66  // Compute sigma - remember to set threshold_ correctly !
67  sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
68  if (debug_verbosity_level > 1)
69  PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);
70 
71  // Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2))
72  Eigen::Vector4f min_pt, max_pt;
73  getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
74  max_pt -= min_pt;
75  double v = sqrt (max_pt.dot (max_pt));
76 
77  int n_inliers_count = 0;
78  std::size_t indices_size;
79  unsigned skipped_count = 0;
80  // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
81  const unsigned max_skip = max_iterations_ * 10;
82 
83  // Iterate
84  while (iterations_ < k && skipped_count < max_skip)
85  {
86  // Get X samples which satisfy the model criteria
87  sac_model_->getSamples (iterations_, selection);
88 
89  if (selection.empty ()) break;
90 
91  // Search for inliers in the point cloud for the current plane model M
92  if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
93  {
94  //iterations_++;
95  ++ skipped_count;
96  continue;
97  }
98 
99  // Iterate through the 3d points and calculate the distances from them to the model
100  sac_model_->getDistancesToModel (model_coefficients, distances);
101 
102  if (distances.empty ())
103  {
104  //iterations_++;
105  ++skipped_count;
106  continue;
107  }
108 
109  // Use Expectiation-Maximization to find out the right value for d_cur_penalty
110  // ---[ Initial estimate for the gamma mixing parameter = 1/2
111  double gamma = 0.5;
112  double p_outlier_prob = 0;
113 
114  indices_size = sac_model_->getIndices ()->size ();
115  std::vector<double> p_inlier_prob (indices_size);
116  for (int j = 0; j < iterations_EM_; ++j)
117  {
118  // Likelihood of a datum given that it is an inlier
119  for (std::size_t i = 0; i < indices_size; ++i)
120  p_inlier_prob[i] = gamma * std::exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
121  (sqrt (2 * M_PI) * sigma_);
122 
123  // Likelihood of a datum given that it is an outlier
124  p_outlier_prob = (1 - gamma) / v;
125 
126  gamma = 0;
127  for (std::size_t i = 0; i < indices_size; ++i)
128  gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob);
129  gamma /= static_cast<double>(sac_model_->getIndices ()->size ());
130  }
131 
132  // Find the std::log likelihood of the model -L = -sum [std::log (pInlierProb + pOutlierProb)]
133  double d_cur_penalty = 0;
134  for (std::size_t i = 0; i < indices_size; ++i)
135  d_cur_penalty += std::log (p_inlier_prob[i] + p_outlier_prob);
136  d_cur_penalty = - d_cur_penalty;
137 
138  // Better match ?
139  if (d_cur_penalty < d_best_penalty)
140  {
141  d_best_penalty = d_cur_penalty;
142 
143  // Save the current model/coefficients selection as being the best so far
144  model_ = selection;
145  model_coefficients_ = model_coefficients;
146 
147  n_inliers_count = 0;
148  // Need to compute the number of inliers for this model to adapt k
149  for (const double &distance : distances)
150  if (distance <= 2 * sigma_)
151  n_inliers_count++;
152 
153  // Compute the k parameter (k=std::log(z)/std::log(1-w^n))
154  double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
155  double p_no_outliers = 1 - std::pow (w, static_cast<double> (selection.size ()));
156  p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by -Inf
157  p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers); // Avoid division by 0.
158  k = std::log (1 - probability_) / std::log (p_no_outliers);
159  }
160 
161  ++iterations_;
162  if (debug_verbosity_level > 1)
163  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);
164  if (iterations_ > max_iterations_)
165  {
166  if (debug_verbosity_level > 0)
167  PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
168  break;
169  }
170  }
171 
172  if (model_.empty ())
173  {
174  if (debug_verbosity_level > 0)
175  PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
176  return (false);
177  }
178 
179  // Iterate through the 3d points and calculate the distances from them to the model again
180  sac_model_->getDistancesToModel (model_coefficients_, distances);
181  Indices &indices = *sac_model_->getIndices ();
182  if (distances.size () != indices.size ())
183  {
184  PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
185  return (false);
186  }
187 
188  inliers_.resize (distances.size ());
189  // Get the inliers for the best model found
190  n_inliers_count = 0;
191  for (std::size_t i = 0; i < distances.size (); ++i)
192  if (distances[i] <= 2 * sigma_)
193  inliers_[n_inliers_count++] = indices[i];
194 
195  // Resize the inliers vector
196  inliers_.resize (n_inliers_count);
197 
198  if (debug_verbosity_level > 0)
199  PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
200 
201  return (true);
202 }
203 
204 //////////////////////////////////////////////////////////////////////////
205 template <typename PointT> double
207  const PointCloudConstPtr &cloud,
208  const IndicesPtr &indices,
209  double sigma) const
210 {
211  std::vector<double> distances (indices->size ());
212 
213  Eigen::Vector4f median;
214  // median (dist (x - median (x)))
215  computeMedian (cloud, indices, median);
216 
217  for (std::size_t i = 0; i < indices->size (); ++i)
218  {
219  pcl::Vector4fMapConst pt = (*cloud)[(*indices)[i]].getVector4fMap ();
220  Eigen::Vector4f ptdiff = pt - median;
221  ptdiff[3] = 0;
222  distances[i] = ptdiff.dot (ptdiff);
223  }
224 
225  std::sort (distances.begin (), distances.end ());
226 
227  double result;
228  std::size_t mid = indices->size () / 2;
229  // Do we have a "middle" point or should we "estimate" one ?
230  if (indices->size () % 2 == 0)
231  result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
232  else
233  result = sqrt (distances[mid]);
234  return (sigma * result);
235 }
236 
237 //////////////////////////////////////////////////////////////////////////
238 template <typename PointT> void
240  const PointCloudConstPtr &cloud,
241  const IndicesPtr &indices,
242  Eigen::Vector4f &min_p,
243  Eigen::Vector4f &max_p) const
244 {
245  min_p.setConstant (FLT_MAX);
246  max_p.setConstant (-FLT_MAX);
247  min_p[3] = max_p[3] = 0;
248 
249  for (std::size_t i = 0; i < indices->size (); ++i)
250  {
251  if ((*cloud)[(*indices)[i]].x < min_p[0]) min_p[0] = (*cloud)[(*indices)[i]].x;
252  if ((*cloud)[(*indices)[i]].y < min_p[1]) min_p[1] = (*cloud)[(*indices)[i]].y;
253  if ((*cloud)[(*indices)[i]].z < min_p[2]) min_p[2] = (*cloud)[(*indices)[i]].z;
254 
255  if ((*cloud)[(*indices)[i]].x > max_p[0]) max_p[0] = (*cloud)[(*indices)[i]].x;
256  if ((*cloud)[(*indices)[i]].y > max_p[1]) max_p[1] = (*cloud)[(*indices)[i]].y;
257  if ((*cloud)[(*indices)[i]].z > max_p[2]) max_p[2] = (*cloud)[(*indices)[i]].z;
258  }
259 }
260 
261 //////////////////////////////////////////////////////////////////////////
262 template <typename PointT> void
264  const PointCloudConstPtr &cloud,
265  const IndicesPtr &indices,
266  Eigen::Vector4f &median) const
267 {
268  // Copy the values to vectors for faster sorting
269  std::vector<float> x (indices->size ());
270  std::vector<float> y (indices->size ());
271  std::vector<float> z (indices->size ());
272  for (std::size_t i = 0; i < indices->size (); ++i)
273  {
274  x[i] = (*cloud)[(*indices)[i]].x;
275  y[i] = (*cloud)[(*indices)[i]].y;
276  z[i] = (*cloud)[(*indices)[i]].z;
277  }
278  std::sort (x.begin (), x.end ());
279  std::sort (y.begin (), y.end ());
280  std::sort (z.begin (), z.end ());
281 
282  std::size_t mid = indices->size () / 2;
283  if (indices->size () % 2 == 0)
284  {
285  median[0] = (x[mid-1] + x[mid]) / 2;
286  median[1] = (y[mid-1] + y[mid]) / 2;
287  median[2] = (z[mid-1] + z[mid]) / 2;
288  }
289  else
290  {
291  median[0] = x[mid];
292  median[1] = y[mid];
293  median[2] = z[mid];
294  }
295  median[3] = 0;
296 }
297 
298 #define PCL_INSTANTIATE_MaximumLikelihoodSampleConsensus(T) template class PCL_EXPORTS pcl::MaximumLikelihoodSampleConsensus<T>;
299 
300 #endif // PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_
301 
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
double computeMedianAbsoluteDeviation(const PointCloudConstPtr &cloud, const IndicesPtr &indices, double sigma) const
Compute the median absolute deviation: .
Definition: mlesac.hpp:206
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
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:408
#define M_PI
Definition: pcl_macros.h:192
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
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:239
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:141
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:263
bool computeModel(int debug_verbosity_level=0) override
Compute the actual model and find the inliers.
Definition: mlesac.hpp:49