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
computeModel(int debug_verbosity_level)50 pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
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
computeMedianAbsoluteDeviation(const PointCloudConstPtr & cloud,const IndicesPtr & indices,double sigma) const209 pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedianAbsoluteDeviation (
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)>(::sqrt));
229 return (sigma * result);
230 }
231
232 //////////////////////////////////////////////////////////////////////////
233 template <typename PointT> void
getMinMax(const PointCloudConstPtr & cloud,const IndicesPtr & indices,Eigen::Vector4f & min_p,Eigen::Vector4f & max_p) const234 pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
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
computeMedian(const PointCloudConstPtr & cloud,const IndicesPtr & indices,Eigen::Vector4f & median) const258 pcl::MaximumLikelihoodSampleConsensus<PointT>::computeMedian (
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
284