Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpRansac.h
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
45 #ifndef vpRANSAC_HH
46 #define vpRANSAC_HH
47 
48 
49 
50 #include <visp3/core/vpUniRand.h> // random number generation
51 #include <visp3/core/vpDebug.h> // debug and trace
52 #include <visp3/core/vpColVector.h>
53 #include <visp3/core/vpMath.h>
54 #include <ctime>
74 template <class vpTransformation>
75 class vpRansac
76 {
77 public:
78  static bool ransac(unsigned int npts,
79  vpColVector &x,
80  unsigned int s, double t,
81  vpColVector &model,
82  vpColVector &inliers,
83  int consensus = 1000,
84  double not_used = 0.0,
85  const int maxNbumbersOfTrials = 10000);
86 };
87 
118 template <class vpTransformation>
119 bool
121  unsigned int s, double t,
122  vpColVector &M,
123  vpColVector &inliers,
124  int consensus,
125  double not_used,
126  const int maxNbumbersOfTrials)
127 {
128  /* bool isplanar; */
129  /* if (s == 4) isplanar = true; */
130  /* else isplanar = false; */
131  (void)not_used;
132  double eps = 1e-6 ;
133  double p = 0.99; // Desired probability of choosing at least one sample
134  // free from outliers
135 
136  int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
137  int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
138  // data set.
139 
140  if (s<4)
141  s = 4;
142 
143  // Sentinel value allowing detection of solution failure.
144  bool solutionFind = false ;
145  vpColVector bestM ;
146  int trialcount = 0;
147  int bestscore = -1;
148  double N = 1; // Dummy initialisation for number of trials.
149 
150  vpUniRand random((const long)time(NULL)) ;
151  vpColVector bestinliers ;
152  unsigned int *ind = new unsigned int [s] ;
153  int numiter = 0;
154  int ninliers = 0;
155  double residual = 0.0;
156  while(( N > trialcount) && (consensus > bestscore))
157  {
158  // Select at random s datapoints to form a trial model, M.
159  // In selecting these points we have to check that they are not in
160  // a degenerate configuration.
161 
162  bool degenerate = true;
163  int count = 1;
164 
165  while ( degenerate == true)
166  {
167  // Generate s random indicies in the range 1..npts
168  for (unsigned int i=0 ; i < s ; i++)
169  ind[i] = (unsigned int)ceil(random()*npts) -1;
170 
171  // Test that these points are not a degenerate configuration.
172  degenerate = vpTransformation::degenerateConfiguration(x,ind) ;
173  // degenerate = feval(degenfn, x(:,ind));
174 
175  // Safeguard against being stuck in this loop forever
176  count = count + 1;
177 
178  if (count > maxDataTrials) {
179  delete [] ind;
180  vpERROR_TRACE("Unable to select a nondegenerate data set");
181  throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
182  //return false; //Useless after a throw() function
183  }
184  }
185  // Fit model to this random selection of data points.
186  vpTransformation::computeTransformation(x,ind, M);
187 
188  vpColVector d ;
189  // Evaluate distances between points and model.
190  vpTransformation::computeResidual(x, M, d) ;
191 
192  // Find the indices of points that are inliers to this model.
193  residual = 0.0;
194  ninliers =0 ;
195  for (unsigned int i=0 ; i < npts ; i++)
196  {
197  double resid = fabs(d[i]);
198  if (resid < t)
199  {
200  inliers[i] = 1 ;
201  ninliers++ ;
202  residual += fabs(d[i]);
203  }
204  else inliers[i] = 0;
205  }
206 
207  if (ninliers > bestscore) // Largest set of inliers so far...
208  {
209  bestscore = ninliers; // Record data for this model
210  bestinliers = inliers;
211  bestM = M;
212  solutionFind = true ;
213 
214  // Update estimate of N, the number of trials to ensure we pick,
215  // with probability p, a data set with no outliers.
216 
217  double fracinliers = (double)ninliers / (double)npts;
218 
219  double pNoOutliers = 1 - pow(fracinliers,static_cast<int>(s));
220 
221  pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
222  pNoOutliers = vpMath::minimum(1-eps, pNoOutliers);// Avoid division by 0.
223  N = (log(1-p)/log(pNoOutliers));
224  }
225 
226  trialcount = trialcount+1;
227  // Safeguard against being stuck in this loop forever
228  if (trialcount > maxTrials)
229  {
230  vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
231  break ;
232  }
233  numiter++;
234  }
235 
236  if (solutionFind==true) // We got a solution
237  {
238  M = bestM;
239  inliers = bestinliers;
240  }
241  else
242  {
243  vpTRACE("ransac was unable to find a useful solution");
244  M = 0;
245  }
246 
247  if(ninliers > 0)
248  residual /= ninliers;
249  delete [] ind;
250 
251  return true;
252 }
253 
254 
255 #endif
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
#define vpTRACE
Definition: vpDebug.h:414
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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, const int maxNbumbersOfTrials=10000)
RANSAC - Robustly fits a model to data with the RANSAC algorithm.
Definition: vpRansac.h:120
This class is a generic implementation of the Ransac algorithm. It cannot be used alone...
Definition: vpRansac.h:75
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:64