41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ 42 #define PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ 44 #include <pcl/sample_consensus/mlesac.h> 45 #include <pcl/point_types.h> 48 template <
typename Po
intT>
bool 52 if (threshold_ == std::numeric_limits<double>::max())
54 PCL_ERROR (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
59 double d_best_penalty = std::numeric_limits<double>::max();
62 std::vector<int> best_model;
63 std::vector<int> selection;
64 Eigen::VectorXf model_coefficients;
65 std::vector<double> distances;
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_);
73 Eigen::Vector4f min_pt, max_pt;
74 getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
76 double v = sqrt (max_pt.dot (max_pt));
78 int n_inliers_count = 0;
80 unsigned skipped_count = 0;
82 const unsigned max_skip = max_iterations_ * 10;
85 while (iterations_ < k && skipped_count < max_skip)
88 sac_model_->getSamples (iterations_, selection);
90 if (selection.empty ())
break;
93 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
101 sac_model_->getDistancesToModel (model_coefficients, distances);
103 if (distances.empty ())
113 double p_outlier_prob = 0;
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)
120 for (
size_t i = 0; i < indices_size; ++i)
121 p_inlier_prob[i] = gamma * exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
122 (sqrt (2 * M_PI) * sigma_);
125 p_outlier_prob = (1 - gamma) / v;
128 for (
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 ());
134 double d_cur_penalty = 0;
135 for (
size_t i = 0; i < indices_size; ++i)
136 d_cur_penalty += log (p_inlier_prob[i] + p_outlier_prob);
137 d_cur_penalty = - d_cur_penalty;
140 if (d_cur_penalty < d_best_penalty)
142 d_best_penalty = d_cur_penalty;
146 model_coefficients_ = model_coefficients;
150 for (
size_t i = 0; i < distances.size (); ++i)
151 if (distances[i] <= 2 * sigma_)
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);
158 p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers);
159 k = log (1 - probability_) / log (p_no_outliers);
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> (ceil (k)), d_best_penalty);
165 if (iterations_ > max_iterations_)
167 if (debug_verbosity_level > 0)
168 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
175 if (debug_verbosity_level > 0)
176 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
181 sac_model_->getDistancesToModel (model_coefficients_, distances);
182 std::vector<int> &indices = *sac_model_->getIndices ();
183 if (distances.size () != indices.size ())
185 PCL_ERROR (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
189 inliers_.resize (distances.size ());
192 for (
size_t i = 0; i < distances.size (); ++i)
193 if (distances[i] <= 2 * sigma_)
194 inliers_[n_inliers_count++] = indices[i];
197 inliers_.resize (n_inliers_count);
199 if (debug_verbosity_level > 0)
200 PCL_DEBUG (
"[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
206 template <
typename Po
intT>
double 208 const PointCloudConstPtr &cloud,
209 const boost::shared_ptr <std::vector<int> > &indices,
212 std::vector<double> distances (indices->size ());
214 Eigen::Vector4f median;
216 computeMedian (cloud, indices, median);
218 for (
size_t i = 0; i < indices->size (); ++i)
221 Eigen::Vector4f ptdiff = pt - median;
223 distances[i] = ptdiff.dot (ptdiff);
226 std::sort (distances.begin (), distances.end ());
229 size_t mid = indices->size () / 2;
231 if (indices->size () % 2 == 0)
232 result = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
234 result = sqrt (distances[mid]);
235 return (sigma * result);
239 template <
typename Po
intT>
void 241 const PointCloudConstPtr &cloud,
242 const boost::shared_ptr <std::vector<int> > &indices,
243 Eigen::Vector4f &min_p,
244 Eigen::Vector4f &max_p)
246 min_p.setConstant (FLT_MAX);
247 max_p.setConstant (-FLT_MAX);
248 min_p[3] = max_p[3] = 0;
250 for (
size_t i = 0; i < indices->size (); ++i)
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;
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;
263 template <
typename Po
intT>
void 265 const PointCloudConstPtr &cloud,
266 const boost::shared_ptr <std::vector<int> > &indices,
267 Eigen::Vector4f &median)
270 std::vector<float> x (indices->size ());
271 std::vector<float> y (indices->size ());
272 std::vector<float> z (indices->size ());
273 for (
size_t i = 0; i < indices->size (); ++i)
275 x[i] = cloud->points[(*indices)[i]].x;
276 y[i] = cloud->points[(*indices)[i]].y;
277 z[i] = cloud->points[(*indices)[i]].z;
279 std::sort (x.begin (), x.end ());
280 std::sort (y.begin (), y.end ());
281 std::sort (z.begin (), z.end ());
283 size_t mid = indices->size () / 2;
284 if (indices->size () % 2 == 0)
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;
299 #define PCL_INSTANTIATE_MaximumLikelihoodSampleConsensus(T) template class PCL_EXPORTS pcl::MaximumLikelihoodSampleConsensus<T>; 301 #endif // PCL_SAMPLE_CONSENSUS_IMPL_MLESAC_H_ double computeMedianAbsoluteDeviation(const PointCloudConstPtr &cloud, const boost::shared_ptr< std::vector< int > > &indices, double sigma)
Compute the median absolute deviation: .
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
void getMinMax(const PointCloudConstPtr &cloud, const boost::shared_ptr< std::vector< int > > &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
Determine the minimum and maximum 3D bounding box coordinates for a given set of points.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
void computeMedian(const PointCloudConstPtr &cloud, const boost::shared_ptr< std::vector< int > > &indices, Eigen::Vector4f &median)
Compute the median value of a 3D point cloud using a given set point indices and return it as a Point...
bool computeModel(int debug_verbosity_level=0)
Compute the actual model and find the inliers.