Visual Servoing Platform version 3.5.0
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
72template <class vpTransformation> class vpRansac
73{
74public:
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
110template <class vpTransformation>
111bool 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
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:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
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, 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
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