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