Main MRPT website > C++ reference for MRPT 1.9.9
ransac.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/ransac.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::random;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 /*---------------------------------------------------------------
21  ransac generic implementation
22  ---------------------------------------------------------------*/
23 template <typename NUMTYPE>
26  const TRansacFitFunctor& fit_func, const TRansacDistanceFunctor& dist_func,
27  const TRansacDegenerateFunctor& degen_func, const double distanceThreshold,
28  const unsigned int minimumSizeSamplesToFit,
29  std::vector<size_t>& out_best_inliers,
30  CMatrixTemplateNumeric<NUMTYPE>& out_best_model, const double p,
31  const size_t maxIter) const
32 {
34 
35  ASSERT_(minimumSizeSamplesToFit >= 1);
36 
37  // Highly inspired on http://www.csse.uwa.edu.au/~pk/
38  const size_t D = data.rows(); // dimensionality
39  const size_t Npts = data.cols();
40 
41  ASSERT_(D >= 1);
42  ASSERT_(Npts > 1);
43 
44  const size_t maxDataTrials =
45  100; // Maximum number of attempts to select a non-degenerate data set.
46 
47  out_best_model.setSize(
48  0, 0); // Sentinel value allowing detection of solution failure.
49  out_best_inliers.clear();
50 
51  size_t trialcount = 0;
52  size_t bestscore = std::string::npos; // npos will mean "none"
53  size_t N = 1; // Dummy initialisation for number of trials.
54 
55  std::vector<size_t> ind(minimumSizeSamplesToFit);
56 
57  while (N > trialcount)
58  {
59  // Select at random s datapoints to form a trial model, M.
60  // In selecting these points we have to check that they are not in
61  // a degenerate configuration.
62  bool degenerate = true;
63  size_t count = 1;
64  std::vector<CMatrixTemplateNumeric<NUMTYPE>> MODELS;
65 
66  while (degenerate)
67  {
68  // Generate s random indicies in the range 1..npts
69  ind.resize(minimumSizeSamplesToFit);
70 
71  // The +0.99... is due to the floor rounding afterwards when
72  // converting from random double samples to size_t
74  ind, 0.0, Npts - 1 + 0.999999);
75 
76  // Test that these points are not a degenerate configuration.
77  degenerate = degen_func(data, ind);
78 
79  if (!degenerate)
80  {
81  // Fit model to this random selection of data points.
82  // Note that M may represent a set of models that fit the data
83  fit_func(data, ind, MODELS);
84 
85  // Depending on your problem it might be that the only way you
86  // can determine whether a data set is degenerate or not is to
87  // try to fit a model and see if it succeeds. If it fails we
88  // reset degenerate to true.
89  degenerate = MODELS.empty();
90  }
91 
92  // Safeguard against being stuck in this loop forever
93  if (++count > maxDataTrials)
94  {
95  MRPT_LOG_WARN("Unable to select a nondegenerate data set");
96  break;
97  }
98  }
99 
100  // Once we are out here we should have some kind of model...
101  // Evaluate distances between points and model returning the indices
102  // of elements in x that are inliers. Additionally, if M is a cell
103  // array of possible models 'distfn' will return the model that has
104  // the most inliers. After this call M will be a non-cell objec
105  // representing only one model.
106  unsigned int bestModelIdx = 1000;
107  std::vector<size_t> inliers;
108  if (!degenerate)
109  {
110  dist_func(
111  data, MODELS, NUMTYPE(distanceThreshold), bestModelIdx,
112  inliers);
113  ASSERT_(bestModelIdx < MODELS.size());
114  }
115 
116  // Find the number of inliers to this model.
117  const size_t ninliers = inliers.size();
118  bool update_estim_num_iters =
119  (trialcount == 0); // Always update on the first iteration,
120  // regardless of the result (even for
121  // ninliers=0)
122 
123  if (ninliers > bestscore ||
124  (bestscore == std::string::npos && ninliers != 0))
125  {
126  bestscore = ninliers; // Record data for this model
127 
128  out_best_model = MODELS[bestModelIdx];
129  out_best_inliers = inliers;
130  update_estim_num_iters = true;
131  }
132 
133  if (update_estim_num_iters)
134  {
135  // Update estimate of N, the number of trials to ensure we pick,
136  // with probability p, a data set with no outliers.
137  double fracinliers = ninliers / static_cast<double>(Npts);
138  double pNoOutliers =
139  1 -
140  pow(fracinliers, static_cast<double>(minimumSizeSamplesToFit));
141 
142  pNoOutliers = std::max(
143  std::numeric_limits<double>::epsilon(),
144  pNoOutliers); // Avoid division by -Inf
145  pNoOutliers = std::min(
146  1.0 - std::numeric_limits<double>::epsilon(),
147  pNoOutliers); // Avoid division by 0.
148  // Number of
149  N = static_cast<size_t>(log(1 - p) / log(pNoOutliers));
151  format(
152  "Iter #%u Estimated number of iters: %u pNoOutliers = %f "
153  "#inliers: %u\n",
154  (unsigned)trialcount, (unsigned)N, pNoOutliers,
155  (unsigned)ninliers));
156  }
157 
158  ++trialcount;
159 
161  format(
162  "trial %u out of %u \r", (unsigned int)trialcount,
163  (unsigned int)ceil(static_cast<double>(N))));
164 
165  // Safeguard against being stuck in this loop forever
166  if (trialcount > maxIter)
167  {
169  format(
170  "Warning: maximum number of trials (%u) reached\n",
171  (unsigned)maxIter));
172  break;
173  }
174  }
175 
176  if (out_best_model.rows() > 0)
177  { // We got a solution
179  format("Finished in %u iterations.\n", (unsigned)trialcount));
180  return true;
181  }
182  else
183  {
184  MRPT_LOG_WARN("Finished without any proper solution!");
185  return false;
186  }
187 
188  MRPT_END
189 }
190 
191 // Template instantiation:
194 
195 #ifdef HAVE_LONG_DOUBLE
197 #endif
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
math-precomp.h
mrpt::math::RANSAC_Template::TRansacFitFunctor
std::function< void(const CMatrixTemplateNumeric< NUMTYPE > &allData, const std::vector< size_t > &useIndices, std::vector< CMatrixTemplateNumeric< NUMTYPE > > &fitModels)> TRansacFitFunctor
The type of the function passed to mrpt::math::ransac - See the documentation for that method for mor...
Definition: ransac.h:39
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::math::CMatrixTemplateNumeric
A matrix of dynamic size.
Definition: CMatrixTemplateNumeric.h:37
data
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3547
count
GLuint GLuint GLsizei count
Definition: glext.h:3528
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::RANSAC_Template::TRansacDegenerateFunctor
std::function< bool(const CMatrixTemplateNumeric< NUMTYPE > &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:53
MRPT_LOG_WARN
#define MRPT_LOG_WARN(_STRING)
Definition: system/COutputLogger.h:431
RandomGenerators.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::math::RANSAC_Template::TRansacDistanceFunctor
std::function< void(const CMatrixTemplateNumeric< NUMTYPE > &allData, const std::vector< CMatrixTemplateNumeric< NUMTYPE > > &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:47
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::math::RANSAC_Template
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:30
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
ransac.h
mrpt::random::CRandomGenerator::drawUniformVector
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
Definition: RandomGenerators.h:137



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST