MRPT  2.0.4
ransac_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
13 #include <cstddef>
14 #include <vector>
15 
16 // To be included from ransac.h only
17 
18 namespace mrpt::math
19 {
20 template <typename NUMTYPE, typename DATASET, typename MODEL>
22  const DATASET& data, const TRansacFitFunctor& fit_func,
23  const TRansacDistanceFunctor& dist_func,
24  const TRansacDegenerateFunctor& degen_func, const double distanceThreshold,
25  const unsigned int minimumSizeSamplesToFit,
26  std::vector<size_t>& out_best_inliers, MODEL& out_best_model,
27  const double p, const size_t maxIter) const
28 {
29  // Highly inspired on http://www.csse.uwa.edu.au/~pk/
31 
32  ASSERT_ABOVEEQ_(minimumSizeSamplesToFit, 1U);
33 
34  const size_t Npts = ransacDatasetSize(data);
35 
36  ASSERT_ABOVE_(Npts, 1);
37 
38  // Maximum number of attempts to select a non-degenerate data set.
39  const size_t maxDataTrials = 100;
40 
41  // Sentinel value allowing detection of solution failure.
42  out_best_model = MODEL();
43  out_best_inliers.clear();
44 
45  size_t trialcount = 0;
46  size_t bestscore = std::string::npos; // npos will mean "none"
47  size_t N = 1; // Dummy initialisation for number of trials.
48 
49  std::vector<size_t> ind(minimumSizeSamplesToFit);
50 
51  while (N > trialcount)
52  {
53  // Select at random s datapoints to form a trial model, M.
54  // In selecting these points we have to check that they are not in
55  // a degenerate configuration.
56  bool degenerate = true;
57  size_t count = 1;
58  std::vector<MODEL> MODELS;
59 
60  while (degenerate)
61  {
62  // Generate s random indicies in the range 1..npts
63  ind.resize(minimumSizeSamplesToFit);
64 
65  // The +0.99... is due to the floor rounding afterwards when
66  // converting from random double samples to size_t
68  ind, 0.0, Npts - 1 + 0.999999);
69 
70  // Test that these points are not a degenerate configuration.
71  degenerate = degen_func(data, ind);
72 
73  if (!degenerate)
74  {
75  // Fit model to this random selection of data points.
76  // Note that M may represent a set of models that fit the data
77  fit_func(data, ind, MODELS);
78 
79  // Depending on your problem it might be that the only way you
80  // can determine whether a data set is degenerate or not is to
81  // try to fit a model and see if it succeeds. If it fails we
82  // reset degenerate to true.
83  degenerate = MODELS.empty();
84  }
85 
86  // Safeguard against being stuck in this loop forever
87  if (++count > maxDataTrials)
88  {
89  MRPT_LOG_WARN("Unable to select a nondegenerate data set");
90  break;
91  }
92  }
93 
94  // Once we are out here we should have some kind of model...
95  // Evaluate distances between points and model returning the indices
96  // of elements in x that are inliers. Additionally, if M is a cell
97  // array of possible models 'distfn' will return the model that has
98  // the most inliers. After this call M will be a non-cell objec
99  // representing only one model.
100  unsigned int bestModelIdx = std::numeric_limits<unsigned int>::max();
101  std::vector<size_t> inliers;
102  if (!degenerate)
103  {
104  dist_func(
105  data, MODELS, static_cast<NUMTYPE>(distanceThreshold),
106  bestModelIdx, inliers);
107  ASSERT_BELOW_(bestModelIdx, MODELS.size());
108  }
109 
110  // Find the number of inliers to this model.
111  const size_t ninliers = inliers.size();
112  // Always update on the first iteration, regardless of the result (even
113  // for ninliers=0)
114  bool update_estim_num_iters = (trialcount == 0);
115 
116  if (ninliers > bestscore ||
117  (bestscore == std::string::npos && ninliers != 0))
118  {
119  bestscore = ninliers; // Record data for this model
120 
121  out_best_model = std::move(MODELS[bestModelIdx]);
122  out_best_inliers = std::move(inliers);
123  update_estim_num_iters = true;
124  }
125 
126  if (update_estim_num_iters)
127  {
128  // Update estimate of N, the number of trials to ensure we pick,
129  // with probability p, a data set with no outliers.
130  double fracinliers = ninliers / static_cast<double>(Npts);
131  double pNoOutliers =
132  1 -
133  pow(fracinliers, static_cast<double>(minimumSizeSamplesToFit));
134 
135  pNoOutliers = std::max(
136  std::numeric_limits<double>::epsilon(),
137  pNoOutliers); // Avoid division by -Inf
138  pNoOutliers = std::min(
139  1.0 - std::numeric_limits<double>::epsilon(),
140  pNoOutliers); // Avoid division by 0.
141  // Number of
142  N = static_cast<size_t>(log(1 - p) / log(pNoOutliers));
144  "Iter #%u Estimated number of iters: %u pNoOutliers = %f "
145  "#inliers: %u",
146  (unsigned)trialcount, (unsigned)N, pNoOutliers,
147  (unsigned)ninliers);
148  }
149 
150  ++trialcount;
151 
153  "trial %u out of %u", (unsigned int)trialcount,
154  (unsigned int)ceil(static_cast<double>(N)));
155 
156  // Safeguard against being stuck in this loop forever
157  if (trialcount > maxIter)
158  {
160  "Warning: maximum number of trials (%u) reached\n",
161  (unsigned)maxIter);
162  break;
163  }
164  }
165 
166  if (!out_best_inliers.empty())
167  { // We got a solution
168  MRPT_LOG_INFO_FMT("Finished in %u iterations.", (unsigned)trialcount);
169  return true;
170  }
171  else
172  {
173  MRPT_LOG_WARN("Finished without any proper solution");
174  return false;
175  }
176 
177  MRPT_END
178 }
179 
180 } // namespace mrpt::math
std::function< void(const DATASET &allData, const std::vector< MODEL > &testModels, const NUMTYPE distanceThreshold, unsigned int &out_bestModelIndex, std::vector< size_t > &out_inlierIndices)> TRansacDistanceFunctor
The type of the function passed to mrpt::math::ransac - See the documentation for that method for mor...
Definition: ransac.h:63
#define MRPT_START
Definition: exceptions.h:241
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
size_t ransacDatasetSize(const CMatrixDynamic< T > &dataset)
Define overloaded functions for user types as required.
Definition: ransac.h:26
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
This base provides a set of functions for maths stuff.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
std::function< void(const DATASET &allData, const std::vector< size_t > &useIndices, std::vector< MODEL > &fitModels)> TRansacFitFunctor
The type of the function passed to mrpt::math::ransac - See the documentation for that method for mor...
Definition: ransac.h:56
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
bool execute(const DATASET &data, const TRansacFitFunctor &fit_func, const TRansacDistanceFunctor &dist_func, const TRansacDegenerateFunctor &degen_func, const double distanceThreshold, const unsigned int minimumSizeSamplesToFit, std::vector< size_t > &out_best_inliers, MODEL &out_best_model, const double prob_good_sample=0.999, const size_t maxIter=2000) const
An implementation of the RANSAC algorithm for robust fitting of models to data.
Definition: ransac_impl.h:21
#define MRPT_LOG_WARN(_STRING)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
std::function< bool(const DATASET &allData, const std::vector< size_t > &useIndices)> TRansacDegenerateFunctor
The type of the function passed to mrpt::math::ransac - See the documentation for that method for mor...
Definition: ransac.h:68
static struct FontData data
Definition: gltext.cpp:144



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020