Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpTemplateTrackerMIESM.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 *
38*****************************************************************************/
39
40#include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
41
42#ifdef VISP_HAVE_OPENMP
43#include <omp.h>
44#endif
45
47 : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false), HDirect(), HInverse(),
48 HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
49{
50 useCompositionnal = false;
51 useInverse = false;
52 if (!Warp->isESMcompatible()) {
53 throw(vpException(vpException::badValue, "The selected warp function is not appropriate for the ESM algorithm..."));
54 }
55}
56
58{
60
61 dW = 0;
62
63 int Nbpoint = 0;
64
65 double i2, j2;
66 double IW, dx, dy;
67 int i, j;
68 int cr, ct;
69 double er, et;
70
71 Nbpoint = 0;
72
73 if (blur)
75
77
78 vpColVector tptemp(nbParam);
79
80 Warp->computeCoeff(p);
81 for (unsigned int point = 0; point < templateSize; point++) {
82 i = ptTemplate[point].y;
83 j = ptTemplate[point].x;
84 X1[0] = j;
85 X1[1] = i;
86
87 Warp->computeDenom(X1, p);
88 Warp->warpX(X1, X2, p);
89
90 j2 = X2[0];
91 i2 = X2[1];
92
93 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
94 Nbpoint++;
95
96 if (blur)
97 IW = BI.getValue(i2, j2);
98 else
99 IW = I.getValue(i2, j2);
100
101 ct = ptTemplateSupp[point].ct;
102 et = ptTemplateSupp[point].et;
103 cr = static_cast<int>((IW * (Nc - 1)) / 255.);
104 er = (IW * (Nc - 1)) / 255. - cr;
105
106 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
107 bspline, ApproxHessian, false);
108 }
109 }
110
111 double MI;
112 computeProba(Nbpoint);
113 computeMI(MI);
115
123 }
124
125 Nbpoint = 0;
127
128 Warp->computeCoeff(p);
129 for (unsigned int point = 0; point < templateSize; point++) {
130 i = ptTemplate[point].y;
131 j = ptTemplate[point].x;
132 X1[0] = j;
133 X1[1] = i;
134
135 Warp->computeDenom(X1, p);
136 Warp->warpX(X1, X2, p);
137
138 j2 = X2[0];
139 i2 = X2[1];
140
141 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight()) && (j2 < I.getWidth())) {
142 Nbpoint++;
143
144 if (!blur)
145 IW = I.getValue(i2, j2);
146 else
147 IW = BI.getValue(i2, j2);
148
149 dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
150 dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
151
152 cr = ptTemplateSupp[point].ct;
153 er = ptTemplateSupp[point].et;
154 ct = static_cast<int>((IW * (Nc - 1)) / 255.);
155 et = (IW * (Nc - 1)) / 255. - ct;
156
157 Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
158
159 for (unsigned int it = 0; it < nbParam; it++)
160 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
161
162 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, tptemp.data, nbParam, bspline,
163 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,
289 }
290 }
291
292 if (Nbpoint == 0) {
293 diverge = true;
294 MI = 0;
295 throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
296 } else {
297 computeProba(Nbpoint);
298 computeMI(MI);
301 }
303 GInverse = G;
304
306 // DIRECT
307
308 Nbpoint = 0;
309 MI = 0;
310
312
313 Warp->computeCoeff(p);
314#ifdef VISP_HAVE_OPENMP
315 int nthreads = omp_get_num_procs();
316 // std::cout << "file: " __FILE__ << " line: " << __LINE__ << "
317 // function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
318 omp_set_num_threads(nthreads);
319#pragma omp parallel for private(i, j, i2, j2) default(shared)
320#endif
321 for (point = 0; point < static_cast<int>(templateSize); point++) {
322 i = ptTemplate[point].y;
323 j = ptTemplate[point].x;
324 X1[0] = j;
325 X1[1] = i;
326 Warp->computeDenom(X1, p);
327 Warp->warpX(i, j, i2, j2, p);
328 X2[0] = j2;
329 X2[1] = i2;
330
331 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
332 Nbpoint++;
333
334 if (!blur)
335 IW = I.getValue(i2, j2);
336 else
337 IW = BI.getValue(i2, j2);
338
339 double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
340 double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
341
342 ct = static_cast<int>((IW * (Nc - 1)) / 255.);
343 et = (IW * (Nc - 1)) / 255. - ct;
344 cr = ptTemplateSupp[point].ct;
345 er = ptTemplateSupp[point].et;
346 Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
347
348 for (unsigned int it = 0; it < nbParam; it++)
349 tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
350
351 vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, tptemp.data, nbParam, bspline,
353 }
354 }
355
356 computeProba(Nbpoint);
357 computeMI(MI);
361 GDirect = G;
362
364 H = HDirect + HInverse;
366 }
367 G = GDirect - GInverse;
368
369 try {
370 if (minimizationMethod == vpTemplateTrackerMIESM::USE_GRADIENT)
371 dp = -gain * 0.3 * G;
372 else {
373 switch (hessianComputation) {
376 break;
378 if (HLM.cond() > HLMdesire.cond()) {
380 } else {
381 dp = gain * 0.3 * HLM.inverseByLU() * G;
382 }
383 break;
384 default:
385 dp = gain * 0.3 * HLM.inverseByLU() * G;
386 break;
387 }
388 }
389 } catch (const vpException &e) {
390 throw(e);
391 }
392
394 dp = -dp;
395
396 if (useBrent) {
397 alpha = 2.;
398 computeOptimalBrentGain(I, p, -MI, dp, alpha);
399 dp = alpha * dp;
400 }
401 p += dp;
402
403 iteration++;
404 }
405 } while ((iteration < iterationMax));
406
410 }
411
412 nbIteration = iteration;
413}
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx)
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition vpImage.h:1592
unsigned int getHeight() const
Definition vpImage.h:184
double cond(double svThreshold=1e-6) const
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
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 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
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize
vpTemplateTrackerPointCompo * ptTemplateCompo
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.