Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
40#ifndef vpRANSAC_HH
41#define vpRANSAC_HH
42
43#include <ctime>
44#include <visp3/core/vpColVector.h>
45#include <visp3/core/vpDebug.h> // debug and trace
46#include <visp3/core/vpMath.h>
47#include <visp3/core/vpUniRand.h> // random number generation
67template <class vpTransformation> class vpRansac
68{
69public:
70 static bool ransac(unsigned int npts, const vpColVector &x, unsigned int s, double t, vpColVector &model,
71 vpColVector &inliers, int consensus = 1000, double not_used = 0.0, int maxNbumbersOfTrials = 10000,
72 double *residual = NULL);
73};
74
107template <class vpTransformation>
108bool vpRansac<vpTransformation>::ransac(unsigned int npts, const vpColVector &x, unsigned int s, double t,
109 vpColVector &M, vpColVector &inliers, int consensus, double not_used,
110 int maxNbumbersOfTrials, double *residual)
111{
112 /* bool isplanar; */
113 /* if (s == 4) isplanar = true; */
114 /* else isplanar = false; */
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 // Sentinel value allowing detection of solution failure.
128 bool solutionFind = false;
129 vpColVector bestM;
130 int trialcount = 0;
131 int bestscore = -1;
132 double N = 1; // Dummy initialisation for number of trials.
133
134 vpUniRand random((const long)time(NULL));
135 vpColVector bestinliers;
136 unsigned int *ind = new unsigned int[s];
137 int ninliers = 0;
138
139 while ((N > trialcount) && (consensus > bestscore)) {
140 // Select at random s data points to form a trial model, M.
141 // In selecting these points we have to check that they are not in
142 // a degenerate configuration.
143
144 bool degenerate = true;
145 int count = 1;
146
147 while (degenerate == true) {
148 // Generate s random indicies in the range 1..npts
149 for (unsigned int i = 0; i < s; i++)
150 ind[i] = (unsigned int)ceil(random() * npts) - 1;
151
152 // Test that these points are not a degenerate configuration.
153 degenerate = vpTransformation::degenerateConfiguration(x, ind);
154 // degenerate = feval(degenfn, x(:,ind));
155
156 // Safeguard against being stuck in this loop forever
157 count = count + 1;
158
159 if (count > maxDataTrials) {
160 delete[] ind;
161 vpERROR_TRACE("Unable to select a nondegenerate data set");
162 throw(vpException(vpException::fatalError, "Unable to select a non degenerate data set"));
163 // return false; //Useless after a throw() function
164 }
165 }
166 // Fit model to this random selection of data points.
167 vpTransformation::computeTransformation(x, ind, M);
168
169 vpColVector d;
170 // Evaluate distances between points and model.
171 vpTransformation::computeResidual(x, M, d);
172
173 // Find the indices of points that are inliers to this model.
174 if (residual != NULL)
175 *residual = 0.0;
176 ninliers = 0;
177 for (unsigned int i = 0; i < npts; i++) {
178 double resid = fabs(d[i]);
179 if (resid < t) {
180 inliers[i] = 1;
181 ninliers++;
182 if (residual != NULL) {
183 *residual += fabs(d[i]);
184 }
185 } else
186 inliers[i] = 0;
187 }
188
189 if (ninliers > bestscore) // Largest set of inliers so far...
190 {
191 bestscore = ninliers; // Record data for this model
192 bestinliers = inliers;
193 bestM = M;
194 solutionFind = true;
195
196 // Update estimate of N, the number of trials to ensure we pick,
197 // with probability p, a data set with no outliers.
198
199 double fracinliers = (double)ninliers / (double)npts;
200
201 double pNoOutliers = 1 - pow(fracinliers, static_cast<int>(s));
202
203 pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
204 pNoOutliers = vpMath::minimum(1 - eps, pNoOutliers); // Avoid division by 0.
205 N = (log(1 - p) / log(pNoOutliers));
206 }
207
208 trialcount = trialcount + 1;
209 // Safeguard against being stuck in this loop forever
210 if (trialcount > maxTrials) {
211 vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
212 break;
213 }
214 }
215
216 if (solutionFind == true) // We got a solution
217 {
218 M = bestM;
219 inliers = bestinliers;
220 } else {
221 vpTRACE("ransac was unable to find a useful solution");
222 M = 0;
223 }
224
225 if (residual != NULL) {
226 if (ninliers > 0) {
227 *residual /= ninliers;
228 }
229 }
230
231 delete[] ind;
232
233 return true;
234}
235
236#endif
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
This class is a generic implementation of the Ransac algorithm. It cannot be used alone.
Definition vpRansac.h:68
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:108
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388