Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/ransac.h>
13 #include <mrpt/random.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::utils;
17 using namespace mrpt::random;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 
22 /*---------------------------------------------------------------
23  ransac generic implementation
24  ---------------------------------------------------------------*/
25 template <typename NUMTYPE>
28  TRansacFitFunctor fit_func,
29  TRansacDistanceFunctor dist_func,
30  TRansacDegenerateFunctor degen_func,
31  const double distanceThreshold,
32  const unsigned int minimumSizeSamplesToFit,
33  mrpt::vector_size_t &out_best_inliers,
34  CMatrixTemplateNumeric<NUMTYPE> &out_best_model,
35  const double p,
36  const size_t maxIter
37  ) const
38 {
40 
41  ASSERT_(minimumSizeSamplesToFit>=1)
42 
43  // Highly inspired on http://www.csse.uwa.edu.au/~pk/
44 
45  const size_t D = size(data,1); // dimensionality
46  const size_t Npts = size(data,2);
47 
48  ASSERT_(D>=1);
49  ASSERT_(Npts>1);
50 
51  const size_t maxDataTrials = 100; // Maximum number of attempts to select a non-degenerate data set.
52 
53  out_best_model.setSize(0,0); // Sentinel value allowing detection of solution failure.
54  out_best_inliers.clear();
55 
56  size_t trialcount = 0;
57  size_t bestscore = std::string::npos; // npos will mean "none"
58  size_t N = 1; // Dummy initialisation for number of trials.
59 
60  vector_size_t ind( minimumSizeSamplesToFit );
61 
62  while (N > trialcount)
63  {
64  // Select at random s datapoints to form a trial model, M.
65  // In selecting these points we have to check that they are not in
66  // a degenerate configuration.
67  bool degenerate=true;
68  size_t count = 1;
69  std::vector< CMatrixTemplateNumeric<NUMTYPE> > MODELS;
70 
71  while (degenerate)
72  {
73  // Generate s random indicies in the range 1..npts
74  ind.resize( minimumSizeSamplesToFit );
75 
76  // The +0.99... is due to the floor rounding afterwards when converting from random double samples to size_t
77  randomGenerator.drawUniformVector(ind,0.0, Npts-1+0.999999 );
78 
79  // Test that these points are not a degenerate configuration.
80  degenerate = degen_func(data, ind);
81 
82  if (!degenerate)
83  {
84  // Fit model to this random selection of data points.
85  // Note that M may represent a set of models that fit the data
86  fit_func(data,ind,MODELS);
87 
88  // Depending on your problem it might be that the only way you
89  // can determine whether a data set is degenerate or not is to
90  // try to fit a model and see if it succeeds. If it fails we
91  // reset degenerate to true.
92  degenerate = MODELS.empty();
93  }
94 
95  // Safeguard against being stuck in this loop forever
96  if (++count > maxDataTrials)
97  {
98  MRPT_LOG_WARN("Unable to select a nondegenerate data set");
99  break;
100  }
101  }
102 
103  // Once we are out here we should have some kind of model...
104  // Evaluate distances between points and model returning the indices
105  // of elements in x that are inliers. Additionally, if M is a cell
106  // array of possible models 'distfn' will return the model that has
107  // the most inliers. After this call M will be a non-cell objec
108  // representing only one model.
109  unsigned int bestModelIdx = 1000;
110  mrpt::vector_size_t inliers;
111  if (!degenerate)
112  {
113  dist_func(data,MODELS, NUMTYPE(distanceThreshold), bestModelIdx, inliers);
114  ASSERT_( bestModelIdx<MODELS.size() );
115  }
116 
117  // Find the number of inliers to this model.
118  const size_t ninliers = inliers.size();
119  bool update_estim_num_iters = (trialcount==0); // Always update on the first iteration, regardless of the result (even for ninliers=0)
120 
121  if (ninliers > bestscore || (bestscore==std::string::npos && ninliers!=0))
122  {
123  bestscore = ninliers; // Record data for this model
124 
125  out_best_model = MODELS[bestModelIdx];
126  out_best_inliers = inliers;
127  update_estim_num_iters=true;
128  }
129 
130  if (update_estim_num_iters)
131  {
132  // Update estimate of N, the number of trials to ensure we pick,
133  // with probability p, a data set with no outliers.
134  double fracinliers = ninliers/static_cast<double>(Npts);
135  double pNoOutliers = 1 - pow(fracinliers,static_cast<double>(minimumSizeSamplesToFit));
136 
137  pNoOutliers = std::max( std::numeric_limits<double>::epsilon(), pNoOutliers); // Avoid division by -Inf
138  pNoOutliers = std::min(1.0 - std::numeric_limits<double>::epsilon() , pNoOutliers); // Avoid division by 0.
139  // Number of
140  N = static_cast<size_t>(log(1 - p) / log(pNoOutliers));
141  MRPT_LOG_DEBUG( format("Iter #%u Estimated number of iters: %u pNoOutliers = %f #inliers: %u\n", (unsigned)trialcount ,(unsigned)N,pNoOutliers, (unsigned)ninliers));
142  }
143 
144  ++trialcount;
145 
146  MRPT_LOG_DEBUG( format("trial %u out of %u \r",(unsigned int)trialcount, (unsigned int)ceil(static_cast<double>(N))) );
147 
148  // Safeguard against being stuck in this loop forever
149  if (trialcount > maxIter)
150  {
151  MRPT_LOG_WARN( format("Warning: maximum number of trials (%u) reached\n", (unsigned)maxIter ));
152  break;
153  }
154  }
155 
156  if (size(out_best_model,1)>0)
157  { // We got a solution
158  MRPT_LOG_INFO(format("Finished in %u iterations.\n",(unsigned)trialcount ));
159  return true;
160  }
161  else
162  {
163  MRPT_LOG_WARN("Finished without any proper solution!");
164  return false;
165  }
166 
167  MRPT_END
168 }
169 
170 
171 
172 
173 // Template instantiation:
176 
177 #ifdef HAVE_LONG_DOUBLE
179 #endif
#define min(a, b)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
#define MRPT_END
#define MRPT_LOG_WARN(_STRING)
#define MRPT_LOG_INFO(_STRING)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
#define MRPT_LOG_DEBUG(_STRING)
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
GLfloat GLfloat p
Definition: glew.h:10113
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
#define ASSERT_(f)
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
GLuint GLuint GLsizei count
Definition: glew.h:1167
A generic RANSAC implementation with models as matrices.
Definition: ransac.h:30
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
std::vector< size_t > vector_size_t



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018