Visual Servoing Platform version 3.5.0
vpTemplateTrackerMIForwardAdditional.cpp
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 * Example of template tracking.
33 *
34 * Authors:
35 * Amaury Dame
36 * Aurelien Yol
37 * Fabien Spindler
38 *
39 *****************************************************************************/
40
41#include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
42
43#ifdef VISP_HAVE_OPENMP
44#include <omp.h>
45#endif
46
48 : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
49{
50 useCompositionnal = false;
51}
52
54{
55 dW = 0;
56
57 int Nbpoint = 0;
58
59 if (blur)
63
64 double Tij;
65 double IW, dx, dy;
66 int cr, ct;
67 double er, et;
68
69 Nbpoint = 0;
70
72 Warp->computeCoeff(p);
73 for (unsigned int point = 0; point < templateSize; point++) {
74 int i = ptTemplate[point].y;
75 int j = ptTemplate[point].x;
76 X1[0] = j;
77 X1[1] = i;
78 X2[0] = j;
79 X2[1] = i;
80
81 Warp->computeDenom(X1, p);
82 Warp->warpX(X1, X2, p);
83
84 double j2 = X2[0];
85 double i2 = X2[1];
86
87 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
88 Nbpoint++;
89 Tij = ptTemplate[point].val;
90 if (!blur)
91 IW = I.getValue(i2, j2);
92 else
93 IW = BI.getValue(i2, j2);
94
95 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
96 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
97
98 ct = static_cast<int>((IW * (Nc - 1)) / 255.);
99 cr = static_cast<int>((Tij * (Nc - 1)) / 255.);
100 et = (IW * (Nc - 1)) / 255. - ct;
101 er = (static_cast<double>(Tij) * (Nc - 1)) / 255. - cr;
102 Warp->dWarp(X1, X2, p, dW);
103
104 double *tptemp = new double[nbParam];
105 for (unsigned int it = 0; it < nbParam; it++)
106 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
107
109 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
111 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
112
113 delete[] tptemp;
114 }
115 }
116
117 if (Nbpoint > 0) {
118 double MI;
119 computeProba(Nbpoint);
120 computeMI(MI);
122
124 try {
126 } catch (const vpException &e) {
127 throw(e);
128 }
129 }
130}
131
133{
134 dW = 0;
135
136 int Nbpoint = 0;
137 if (blur)
141
142 double MI = 0, MIprec = -1000;
143
145
146 double alpha = 2.;
147
148 unsigned int iteration = 0;
149
151 double evolRMS_init = 0;
152 double evolRMS_prec = 0;
153 double evolRMS_delta;
154
155 do {
156 if (iteration % 5 == 0)
158 Nbpoint = 0;
159 MIprec = MI;
160 MI = 0;
161 // erreur=0;
162
164
165 Warp->computeCoeff(p);
166#ifdef VISP_HAVE_OPENMP
167 int nthreads = omp_get_num_procs();
168 // std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function:
169 // " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
170 omp_set_num_threads(nthreads);
171#pragma omp parallel for default(shared)
172#endif
173 for (int point = 0; point < (int)templateSize; point++) {
174 int i = ptTemplate[point].y;
175 int j = ptTemplate[point].x;
176 X1[0] = j;
177 X1[1] = i;
178
179 Warp->computeDenom(X1, p);
180 Warp->warpX(X1, X2, p);
181
182 double j2 = X2[0];
183 double i2 = X2[1];
184
185 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
186 Nbpoint++;
187 double Tij = ptTemplate[point].val;
188 double IW;
189 if (!blur)
190 IW = I.getValue(i2, j2);
191 else
192 IW = BI.getValue(i2, j2);
193
194 double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
195 double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
196
197 int ct = (int)((IW * (Nc - 1)) / 255.);
198 int cr = (int)((Tij * (Nc - 1)) / 255.);
199 double et = (IW * (Nc - 1)) / 255. - ct;
200 double er = ((double)Tij * (Nc - 1)) / 255. - cr;
201
202 Warp->dWarp(X1, X2, p, dW);
203
204 double *tptemp = new double[nbParam];
205 for (unsigned int it = 0; it < nbParam; it++)
206 tptemp[it] = (dW[0][it] * dx + dW[1][it] * dy);
208 vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
210 vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
211
212 delete[] tptemp;
213 }
214 }
215
216 if (Nbpoint == 0) {
217 diverge = true;
218 MI = 0;
219 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
220 } else {
221 computeProba(Nbpoint);
222 computeMI(MI);
225
227 try {
228 switch (hessianComputation) {
231 break;
233 if (HLM.cond() > HLMdesire.cond())
235 else
236 dp = gain * 0.2 * HLM.inverseByLU() * G;
237 break;
238 default:
239 dp = gain * 0.2 * HLM.inverseByLU() * G;
240 break;
241 }
242 } catch (const vpException &e) {
243 throw(e);
244 }
245 }
246
247 switch (minimizationMethod) {
249 vpColVector p_test_LMA(nbParam);
251 p_test_LMA = p - 100000.1 * dp;
252 else
253 p_test_LMA = p + dp;
254 MI = -getCost(I, p);
255 double MI_LMA = -getCost(I, p_test_LMA);
256 if (MI_LMA > MI) {
257 p = p_test_LMA;
258 lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
259 } else {
260 lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
261 }
262 } break;
264 dp = -gain * 6.0 * G;
265 if (useBrent) {
266 alpha = 2.;
267 computeOptimalBrentGain(I, p, -MI, dp, alpha);
268 dp = alpha * dp;
269 }
270 p += dp;
271 break;
272 }
273
275 if (iterationGlobale != 0) {
276 vpColVector s_quasi = p - p_prec;
277 vpColVector y_quasi = G - G_prec;
278 double s_scal_y = s_quasi.t() * y_quasi;
279 if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
280 KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.t() / s_scal_y -
281 KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
282 (y_quasi.t() * KQuasiNewton * y_quasi));
283 }
284 }
285 dp = -KQuasiNewton * G;
286 p_prec = p;
287 G_prec = G;
288 p -= 1.01 * dp;
289 } break;
290
291 default: {
293 dp = -0.1 * dp;
294 if (useBrent) {
295 alpha = 2.;
296 computeOptimalBrentGain(I, p, -MI, dp, alpha);
297 dp = alpha * dp;
298 }
299
300 p += dp;
301 break;
302 }
303 }
304
306
307 if (iteration == 0) {
308 evolRMS_init = evolRMS;
309 }
310 iteration++;
312
313 evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
314 evolRMS_prec = evolRMS;
315
316 } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
317 (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init)*evolRMS_eps));
318
319 if (Nbpoint == 0) {
320 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
321 }
322
323 nbIteration = iteration;
327 }
328}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
unsigned int getWidth() const
Definition: vpImage.h:246
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
unsigned int getHeight() const
Definition: vpImage.h:188
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
void initHessienDesired(const vpImage< unsigned char > &I)
void trackNoPyr(const vpImage< unsigned char > &I)
vpHessienApproximationType ApproxHessian
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
vpImage< double > dIx
vpImage< double > dIy
void computeEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int iterationGlobale
vpImage< double > BI
unsigned int templateSize
Error that can be emited by the vpTracker class and its derivates.