Visual Servoing Platform  version 3.5.1 under development (2022-11-27)
vpRansac.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Ransac robust algorithm.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #ifndef vpRANSAC_HH
46 #define vpRANSAC_HH
47 
48 #include <ctime>
49 #include <visp3/core/vpColVector.h>
50 #include <visp3/core/vpDebug.h> // debug and trace
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpUniRand.h> // random number generation
72 template <class vpTransformation> class vpRansac
73 {
74 public:
75  static bool ransac(unsigned int npts, const vpColVector &x, unsigned int s, double t, vpColVector &model,
76  vpColVector &inliers, int consensus = 1000, double not_used = 0.0, int maxNbumbersOfTrials = 10000,
77  double *residual = NULL);
78 };
79 
112 template <class vpTransformation>
113 bool vpRansac<vpTransformation>::ransac(unsigned int npts, const vpColVector &x, unsigned int s, double t,
114  vpColVector &M, vpColVector &inliers, int consensus, double not_used,
115  int maxNbumbersOfTrials, double *residual)
116 {
117  /* bool isplanar; */
118  /* if (s == 4) isplanar = true; */
119  /* else isplanar = false; */
120  (void)not_used;
121  double eps = 1e-6;
122  double p = 0.99; // Desired probability of choosing at least one sample
123  // free from outliers
124 
125  int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
126  int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
127  // data set.
128 
129  if (s < 4)
130  s = 4;
131 
132  // Sentinel value allowing detection of solution failure.
133  bool solutionFind = false;
134  vpColVector bestM;
135  int trialcount = 0;
136  int bestscore = -1;
137  double N = 1; // Dummy initialisation for number of trials.
138 
139  vpUniRand random((const long)time(NULL));
140  vpColVector bestinliers;
141  unsigned int *ind = new unsigned int[s];
142  int numiter = 0;
143  int ninliers = 0;
144 
145  while ((N > trialcount) && (consensus > bestscore)) {
146  // Select at random s datapoints to form a trial model, M.
147  // In selecting these points we have to check that they are not in
148  // a degenerate configuration.
149 
150  bool degenerate = true;
151  int count = 1;
152 
153  while (degenerate == true) {
154  // Generate s random indicies in the range 1..npts
155  for (unsigned int i = 0; i < s; i++)
156  ind[i] = (unsigned int)ceil(random() * npts) - 1;
157 
158  // Test that these points are not a degenerate configuration.
159  degenerate = vpTransformation::degenerateConfiguration(x, ind);
160  // degenerate = feval(degenfn, x(:,ind));
161 
162  // Safeguard against being stuck in this loop forever
163  count = count + 1;
164 
165  if (count > maxDataTrials) {
166  delete[] ind;
167  vpERROR_TRACE("Unable to select a nondegenerate data set");
168  throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
169  // return false; //Useless after a throw() function
170  }
171  }
172  // Fit model to this random selection of data points.
173  vpTransformation::computeTransformation(x, ind, M);
174 
175  vpColVector d;
176  // Evaluate distances between points and model.
177  vpTransformation::computeResidual(x, M, d);
178 
179  // Find the indices of points that are inliers to this model.
180  if (residual != NULL)
181  *residual = 0.0;
182  ninliers = 0;
183  for (unsigned int i = 0; i < npts; i++) {
184  double resid = fabs(d[i]);
185  if (resid < t) {
186  inliers[i] = 1;
187  ninliers++;
188  if (residual != NULL) {
189  *residual += fabs(d[i]);
190  }
191  } else
192  inliers[i] = 0;
193  }
194 
195  if (ninliers > bestscore) // Largest set of inliers so far...
196  {
197  bestscore = ninliers; // Record data for this model
198  bestinliers = inliers;
199  bestM = M;
200  solutionFind = true;
201 
202  // Update estimate of N, the number of trials to ensure we pick,
203  // with probability p, a data set with no outliers.
204 
205  double fracinliers = (double)ninliers / (double)npts;
206 
207  double pNoOutliers = 1 - pow(fracinliers, static_cast<int>(s));
208 
209  pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
210  pNoOutliers = vpMath::minimum(1 - eps, pNoOutliers); // Avoid division by 0.
211  N = (log(1 - p) / log(pNoOutliers));
212  }
213 
214  trialcount = trialcount + 1;
215  // Safeguard against being stuck in this loop forever
216  if (trialcount > maxTrials) {
217  vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
218  break;
219  }
220  numiter++;
221  }
222 
223  if (solutionFind == true) // We got a solution
224  {
225  M = bestM;
226  inliers = bestinliers;
227  } else {
228  vpTRACE("ransac was unable to find a useful solution");
229  M = 0;
230  }
231 
232  if (residual != NULL) {
233  if (ninliers > 0) {
234  *residual /= ninliers;
235  }
236  }
237 
238  delete[] ind;
239 
240  return true;
241 }
242 
243 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:171
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:179
This class is a generic implementation of the Ransac algorithm. It cannot be used alone.
Definition: vpRansac.h:73
static bool ransac(unsigned int npts, const vpColVector &x, unsigned int s, double t, vpColVector &model, vpColVector &inliers, int consensus=1000, double not_used=0.0, int maxNbumbersOfTrials=10000, double *residual=NULL)
RANSAC - Robustly fits a model to data with the RANSAC algorithm.
Definition: vpRansac.h:113
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393