Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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, vpColVector &x, unsigned int s, double t, vpColVector &model,
76  vpColVector &inliers, int consensus = 1000, double not_used = 0.0,
77  int maxNbumbersOfTrials = 10000);
78 };
79 
110 template <class vpTransformation>
111 bool vpRansac<vpTransformation>::ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &M,
112  vpColVector &inliers, int consensus, double not_used,
113  int maxNbumbersOfTrials)
114 {
115  /* bool isplanar; */
116  /* if (s == 4) isplanar = true; */
117  /* else isplanar = false; */
118  (void)not_used;
119  double eps = 1e-6;
120  double p = 0.99; // Desired probability of choosing at least one sample
121  // free from outliers
122 
123  int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
124  int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
125  // data set.
126 
127  if (s < 4)
128  s = 4;
129 
130  // Sentinel value allowing detection of solution failure.
131  bool solutionFind = false;
132  vpColVector bestM;
133  int trialcount = 0;
134  int bestscore = -1;
135  double N = 1; // Dummy initialisation for number of trials.
136 
137  vpUniRand random((const long)time(NULL));
138  vpColVector bestinliers;
139  unsigned int *ind = new unsigned int[s];
140  int numiter = 0;
141  int ninliers = 0;
142  double residual = 0.0;
143  while ((N > trialcount) && (consensus > bestscore)) {
144  // Select at random s datapoints to form a trial model, M.
145  // In selecting these points we have to check that they are not in
146  // a degenerate configuration.
147 
148  bool degenerate = true;
149  int count = 1;
150 
151  while (degenerate == true) {
152  // Generate s random indicies in the range 1..npts
153  for (unsigned int i = 0; i < s; i++)
154  ind[i] = (unsigned int)ceil(random() * npts) - 1;
155 
156  // Test that these points are not a degenerate configuration.
157  degenerate = vpTransformation::degenerateConfiguration(x, ind);
158  // degenerate = feval(degenfn, x(:,ind));
159 
160  // Safeguard against being stuck in this loop forever
161  count = count + 1;
162 
163  if (count > maxDataTrials) {
164  delete[] ind;
165  vpERROR_TRACE("Unable to select a nondegenerate data set");
166  throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
167  // return false; //Useless after a throw() function
168  }
169  }
170  // Fit model to this random selection of data points.
171  vpTransformation::computeTransformation(x, ind, M);
172 
173  vpColVector d;
174  // Evaluate distances between points and model.
175  vpTransformation::computeResidual(x, M, d);
176 
177  // Find the indices of points that are inliers to this model.
178  residual = 0.0;
179  ninliers = 0;
180  for (unsigned int i = 0; i < npts; i++) {
181  double resid = fabs(d[i]);
182  if (resid < t) {
183  inliers[i] = 1;
184  ninliers++;
185  residual += fabs(d[i]);
186  } else
187  inliers[i] = 0;
188  }
189 
190  if (ninliers > bestscore) // Largest set of inliers so far...
191  {
192  bestscore = ninliers; // Record data for this model
193  bestinliers = inliers;
194  bestM = M;
195  solutionFind = true;
196 
197  // Update estimate of N, the number of trials to ensure we pick,
198  // with probability p, a data set with no outliers.
199 
200  double fracinliers = (double)ninliers / (double)npts;
201 
202  double pNoOutliers = 1 - pow(fracinliers, static_cast<int>(s));
203 
204  pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
205  pNoOutliers = vpMath::minimum(1 - eps, pNoOutliers); // Avoid division by 0.
206  N = (log(1 - p) / log(pNoOutliers));
207  }
208 
209  trialcount = trialcount + 1;
210  // Safeguard against being stuck in this loop forever
211  if (trialcount > maxTrials) {
212  vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
213  break;
214  }
215  numiter++;
216  }
217 
218  if (solutionFind == true) // We got a solution
219  {
220  M = bestM;
221  inliers = bestinliers;
222  } else {
223  vpTRACE("ransac was unable to find a useful solution");
224  M = 0;
225  }
226 
227  if (ninliers > 0)
228  residual /= ninliers;
229  delete[] ind;
230 
231  return true;
232 }
233 
234 #endif
#define vpERROR_TRACE
Definition: vpDebug.h:393
error that can be emited by ViSP classes.
Definition: vpException.h:71
static bool ransac(unsigned int npts, vpColVector &x, unsigned int s, double t, vpColVector &model, vpColVector &inliers, int consensus=1000, double not_used=0.0, int maxNbumbersOfTrials=10000)
RANSAC - Robustly fits a model to data with the RANSAC algorithm.
Definition: vpRansac.h:111
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
#define vpTRACE
Definition: vpDebug.h:416
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
This class is a generic implementation of the Ransac algorithm. It cannot be used alone...
Definition: vpRansac.h:72
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:100