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