Visual Servoing Platform version 3.5.0
vpTemplateTrackerMIESM.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/vpTemplateTrackerMIESM.h>
42
43#ifdef VISP_HAVE_OPENMP
44#include <omp.h>
45#endif
46
48 : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false), HDirect(), HInverse(),
49 HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
50{
51 useCompositionnal = false;
52 useInverse = false;
53 if (!Warp->isESMcompatible()) {
55 "The selected warp function is not appropriate for the ESM algorithm..."));
56 }
57}
58
60{
62
63 dW = 0;
64
65 int Nbpoint = 0;
66
67 double i2, j2;
68 double IW, dx, dy;
69 int i, j;
70 int cr, ct;
71 double er, et;
72
73 Nbpoint = 0;
74
75 if (blur)
77
79
80 vpColVector tptemp(nbParam);
81
82 Warp->computeCoeff(p);
83 for (unsigned int point = 0; point < templateSize; point++) {
84 i = ptTemplate[point].y;
85 j = ptTemplate[point].x;
86 X1[0] = j;
87 X1[1] = i;
88
89 Warp->computeDenom(X1, p);
90 Warp->warpX(X1, X2, p);
91
92 j2 = X2[0];
93 i2 = X2[1];
94
95 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
96 Nbpoint++;
97
98 if (blur)
99 IW = BI.getValue(i2,j2);
100 else
101 IW = I.getValue(i2,j2);
102
103 ct = ptTemplateSupp[point].ct;
104 et = ptTemplateSupp[point].et;
105 cr = static_cast<int>((IW*(Nc-1))/255.);
106 er = (IW*(Nc-1))/255.-cr;
107
108 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline, ApproxHessian, false);
109 }
110 }
111
112 double MI;
113 computeProba(Nbpoint);
114 computeMI(MI);
116
124 }
125
126 Nbpoint = 0;
128
129 Warp->computeCoeff(p);
130 for (unsigned int point = 0; point < templateSize; point++) {
131 i = ptTemplate[point].y;
132 j = ptTemplate[point].x;
133 X1[0] = j;
134 X1[1] = i;
135
136 Warp->computeDenom(X1, p);
137 Warp->warpX(X1, X2, p);
138
139 j2 = X2[0];
140 i2 = X2[1];
141
142 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight()) && (j2 < I.getWidth())) {
143 Nbpoint++;
144
145 if(!blur)
146 IW=I.getValue(i2,j2);
147 else
148 IW=BI.getValue(i2,j2);
149
150 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
151 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
152
153 cr = ptTemplateSupp[point].ct;
154 er = ptTemplateSupp[point].et;
155 ct = static_cast<int>((IW*(Nc-1))/255.);
156 et = (IW*(Nc-1))/255.-ct;
157
158 Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
159
160 for (unsigned int it = 0; it < nbParam; it++)
161 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
162
163 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, tptemp.data, nbParam, bspline, ApproxHessian, false);
164 }
165 }
166
167 computeProba(Nbpoint);
168 computeMI(MI);
170
172
174
176
178}
179
181{
188
189 ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
191 for (unsigned int point = 0; point < templateSize; point++) {
192 int i = ptTemplate[point].y;
193 int j = ptTemplate[point].x;
194 X1[0] = j;
195 X1[1] = i;
196 Warp->computeDenom(X1, p);
197
198 ptTemplateCompo[point].dW = new double[2 * nbParam];
199 Warp->getdWdp0(i, j, ptTemplateCompo[point].dW);
200
201 ptTemplate[point].dW = new double[nbParam];
202 double dx = ptTemplate[point].dx * (Nc - 1) / 255.;
203 double dy = ptTemplate[point].dy * (Nc - 1) / 255.;
204 Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
205
206 double Tij = ptTemplate[point].val;
207 int ct = static_cast<int>((Tij * (Nc - 1)) / 255.);
208 double et = (Tij * (Nc - 1)) / 255. - ct;
209 ptTemplateSupp[point].et = et;
210 ptTemplateSupp[point].ct = ct;
211 ptTemplateSupp[point].Bt = new double[4];
212 ptTemplateSupp[point].dBt = new double[4];
213 for (char it = -1; it <= 2; it++) {
214 ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
215 ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
216 }
217 }
218 CompoInitialised = true;
219}
220
222{
223 if (!CompoInitialised) {
224 std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
225 }
226 dW = 0;
227
228 if (blur)
232
233 int point;
234
236
238
239 double i2, j2;
240 double IW;
241 int cr, ct;
242 double er, et;
243
244 vpColVector dpinv(nbParam);
245
246 double alpha = 2.;
247
248 int i, j;
249 unsigned int iteration = 0;
250 vpColVector tptemp(nbParam);
251
252 do {
253 int Nbpoint = 0;
254 double MI = 0;
255
257
259 // Inverse
260 Warp->computeCoeff(p);
261 for (point = 0; point < static_cast<int>(templateSize); point++) {
262 i = ptTemplate[point].y;
263 j = ptTemplate[point].x;
264 X1[0] = j;
265 X1[1] = i;
266
267 Warp->computeDenom(X1, p);
268 Warp->warpX(X1, X2, p);
269
270 j2 = X2[0];
271 i2 = X2[1];
272
273 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
274 Nbpoint++;
275
276 if(!blur)
277 IW=I.getValue(i2,j2);
278 else
279 IW=BI.getValue(i2,j2);
280
281 ct = ptTemplateSupp[point].ct;
282 et = ptTemplateSupp[point].et;
283 cr = static_cast<int>((IW*(Nc-1))/255.);
284 er = (IW*(Nc-1))/255.-cr;
285
286 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline, ApproxHessian, hessianComputation==USE_HESSIEN_DESIRE);
287 }
288 }
289
290 if (Nbpoint == 0) {
291 diverge = true;
292 MI = 0;
293 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
294 } else {
295 computeProba(Nbpoint);
296 computeMI(MI);
299 }
301 GInverse = G;
302
304 // DIRECT
305
306 Nbpoint = 0;
307 MI = 0;
308
310
311 Warp->computeCoeff(p);
312#ifdef VISP_HAVE_OPENMP
313 int nthreads = omp_get_num_procs();
314 // std::cout << "file: " __FILE__ << " line: " << __LINE__ << "
315 // function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
316 omp_set_num_threads(nthreads);
317#pragma omp parallel for private(point, i, j, i2, j2) default(shared)
318#endif
319 for (point = 0; point < static_cast<int>(templateSize); point++) {
320 i = ptTemplate[point].y;
321 j = ptTemplate[point].x;
322 X1[0] = j;
323 X1[1] = i;
324 Warp->computeDenom(X1, p);
325 Warp->warpX(i, j, i2, j2, p);
326 X2[0] = j2;
327 X2[1] = i2;
328
329 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
330 Nbpoint++;
331
332 if(!blur)
333 IW=I.getValue(i2,j2);
334 else
335 IW=BI.getValue(i2,j2);
336
337 double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
338 double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
339
340 ct = static_cast<int>((IW*(Nc-1))/255.);
341 et = (IW*(Nc-1))/255.-ct;
342 cr = ptTemplateSupp[point].ct;
343 er = ptTemplateSupp[point].et;
344 Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
345
346 for (unsigned int it = 0; it < nbParam; it++)
347 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
348
349 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp.data, nbParam, bspline, ApproxHessian, hessianComputation==USE_HESSIEN_DESIRE);
350 }
351 }
352
353 computeProba(Nbpoint);
354 computeMI(MI);
358 GDirect = G;
359
361 H = HDirect + HInverse;
363 }
364 G = GDirect - GInverse;
365
366 try {
367 if (minimizationMethod == vpTemplateTrackerMIESM::USE_GRADIENT)
368 dp = -gain * 0.3 * G;
369 else {
370 switch (hessianComputation) {
373 break;
375 if (HLM.cond() > HLMdesire.cond()) {
377 }
378 else {
379 dp = gain * 0.3 * HLM.inverseByLU() * G;
380 }
381 break;
382 default:
383 dp = gain * 0.3 * HLM.inverseByLU() * G;
384 break;
385 }
386 }
387 } catch (const vpException &e) {
388 throw(e);
389 }
390
392 dp = - dp;
393
394 if (useBrent) {
395 alpha = 2.;
396 computeOptimalBrentGain(I, p, -MI, dp, alpha);
397 dp = alpha * dp;
398 }
399 p += dp;
400
401 iteration++;
402 }
403 } while ((iteration < iterationMax));
404
408 }
409
410 nbIteration = iteration;
411}
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
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 getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
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 trackNoPyr(const vpImage< unsigned char > &I)
void initHessienDesired(const vpImage< unsigned char > &I)
vpMinimizationTypeMIESM minimizationMethod
vpTemplateTrackerMIESM()
Default constructor.
vpHessienApproximationType ApproxHessian
vpImage< double > d2Ix
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
vpImage< double > d2Ixy
vpImage< double > d2Iy
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, const double *dwdp0, vpMatrix &dM)=0
virtual bool isESMcompatible() const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
virtual void getdWdp0(const int &v, const int &u, double *dIdW)=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 computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize
vpTemplateTrackerPointCompo * ptTemplateCompo
Error that can be emited by the vpTracker class and its derivates.