Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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 
171  lambda = lambdaDep;
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 
235  MI_preEstimation = -getCost(I, p);
236 
237  lambda = lambdaDep;
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  }
302  computeGradient();
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);
360  computeGradient();
361  GDirect = G;
362 
364  H = HDirect + HInverse;
366  }
367  G = GDirect - GInverse;
368 
369  try {
371  dp = -gain * 0.3 * G;
372  else {
373  switch (hessianComputation) {
375  dp = gain * HLMdesireInverse * G;
376  break;
378  if (HLM.cond() > HLMdesire.cond()) {
379  dp = gain * HLMdesireInverse * G;
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 
407  MI_postEstimation = -getCost(I, p);
409  MI_postEstimation = -1;
410  }
411 
412  nbIteration = iteration;
413 }
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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 getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx, const vpImage< bool > *p_mask=nullptr)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false, const vpImage< bool > *p_mask=nullptr)
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy, const vpImage< bool > *p_mask=nullptr)
unsigned int getWidth() const
Definition: vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1567
unsigned int getHeight() const
Definition: vpImage.h:184
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6610
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6671
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)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) vp_override
void computeHessien(vpMatrix &H)
vpImage< double > d2Ixy
vpImage< double > d2Iy
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 emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.