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