Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
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()) {
54  throw(vpException(vpException::badValue, "The selected warp function is not appropriate for the ESM algorithm..."));
55  }
56 }
57 
59 {
61 
62  dW = 0;
63 
64  int Nbpoint = 0;
65 
66  double i2, j2;
67  double IW, dx, dy;
68  int i, j;
69  int cr, ct;
70  double er, et;
71 
72  Nbpoint = 0;
73 
74  if (blur)
76 
78 
79  vpColVector tptemp(nbParam);
80 
81  Warp->computeCoeff(p);
82  for (unsigned int point = 0; point < templateSize; point++) {
83  i = ptTemplate[point].y;
84  j = ptTemplate[point].x;
85  X1[0] = j;
86  X1[1] = i;
87 
88  Warp->computeDenom(X1, p);
89  Warp->warpX(X1, X2, p);
90 
91  j2 = X2[0];
92  i2 = X2[1];
93 
94  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
95  Nbpoint++;
96 
97  if (blur)
98  IW = BI.getValue(i2, j2);
99  else
100  IW = I.getValue(i2, j2);
101 
102  ct = ptTemplateSupp[point].ct;
103  et = ptTemplateSupp[point].et;
104  cr = static_cast<int>((IW * (Nc - 1)) / 255.);
105  er = (IW * (Nc - 1)) / 255. - cr;
106 
107  vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
108  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,
164  ApproxHessian, false);
165  }
166  }
167 
168  computeProba(Nbpoint);
169  computeMI(MI);
171 
172  lambda = lambdaDep;
173 
175 
177 
179 }
180 
182 {
189 
190  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
192  for (unsigned int point = 0; point < templateSize; point++) {
193  int i = ptTemplate[point].y;
194  int j = ptTemplate[point].x;
195  X1[0] = j;
196  X1[1] = i;
197  Warp->computeDenom(X1, p);
198 
199  ptTemplateCompo[point].dW = new double[2 * nbParam];
200  Warp->getdWdp0(i, j, ptTemplateCompo[point].dW);
201 
202  ptTemplate[point].dW = new double[nbParam];
203  double dx = ptTemplate[point].dx * (Nc - 1) / 255.;
204  double dy = ptTemplate[point].dy * (Nc - 1) / 255.;
205  Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
206 
207  double Tij = ptTemplate[point].val;
208  int ct = static_cast<int>((Tij * (Nc - 1)) / 255.);
209  double et = (Tij * (Nc - 1)) / 255. - ct;
210  ptTemplateSupp[point].et = et;
211  ptTemplateSupp[point].ct = ct;
212  ptTemplateSupp[point].Bt = new double[4];
213  ptTemplateSupp[point].dBt = new double[4];
214  for (char it = -1; it <= 2; it++) {
215  ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
216  ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
217  }
218  }
219  CompoInitialised = true;
220 }
221 
223 {
224  if (!CompoInitialised) {
225  std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
226  }
227  dW = 0;
228 
229  if (blur)
233 
234  int point;
235 
236  MI_preEstimation = -getCost(I, p);
237 
238  lambda = lambdaDep;
239 
240  double i2, j2;
241  double IW;
242  int cr, ct;
243  double er, et;
244 
245  vpColVector dpinv(nbParam);
246 
247  double alpha = 2.;
248 
249  int i, j;
250  unsigned int iteration = 0;
251  vpColVector tptemp(nbParam);
252 
253  do {
254  int Nbpoint = 0;
255  double MI = 0;
256 
258 
260  // Inverse
261  Warp->computeCoeff(p);
262  for (point = 0; point < static_cast<int>(templateSize); point++) {
263  i = ptTemplate[point].y;
264  j = ptTemplate[point].x;
265  X1[0] = j;
266  X1[1] = i;
267 
268  Warp->computeDenom(X1, p);
269  Warp->warpX(X1, X2, p);
270 
271  j2 = X2[0];
272  i2 = X2[1];
273 
274  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
275  Nbpoint++;
276 
277  if (!blur)
278  IW = I.getValue(i2, j2);
279  else
280  IW = BI.getValue(i2, j2);
281 
282  ct = ptTemplateSupp[point].ct;
283  et = ptTemplateSupp[point].et;
284  cr = static_cast<int>((IW * (Nc - 1)) / 255.);
285  er = (IW * (Nc - 1)) / 255. - cr;
286 
287  vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
290  }
291  }
292 
293  if (Nbpoint == 0) {
294  diverge = true;
295  MI = 0;
296  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
297  } else {
298  computeProba(Nbpoint);
299  computeMI(MI);
302  }
303  computeGradient();
304  GInverse = G;
305 
307  // DIRECT
308 
309  Nbpoint = 0;
310  MI = 0;
311 
313 
314  Warp->computeCoeff(p);
315 #ifdef VISP_HAVE_OPENMP
316  int nthreads = omp_get_num_procs();
317  // std::cout << "file: " __FILE__ << " line: " << __LINE__ << "
318  // function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
319  omp_set_num_threads(nthreads);
320 #pragma omp parallel for private(i, j, i2, j2) default(shared)
321 #endif
322  for (point = 0; point < static_cast<int>(templateSize); point++) {
323  i = ptTemplate[point].y;
324  j = ptTemplate[point].x;
325  X1[0] = j;
326  X1[1] = i;
327  Warp->computeDenom(X1, p);
328  Warp->warpX(i, j, i2, j2, p);
329  X2[0] = j2;
330  X2[1] = i2;
331 
332  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
333  Nbpoint++;
334 
335  if (!blur)
336  IW = I.getValue(i2, j2);
337  else
338  IW = BI.getValue(i2, j2);
339 
340  double dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
341  double dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
342 
343  ct = static_cast<int>((IW * (Nc - 1)) / 255.);
344  et = (IW * (Nc - 1)) / 255. - ct;
345  cr = ptTemplateSupp[point].ct;
346  er = ptTemplateSupp[point].et;
347  Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
348 
349  for (unsigned int it = 0; it < nbParam; it++)
350  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
351 
352  vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout, cr, er, ct, et, Nc, tptemp.data, nbParam, bspline,
354  }
355  }
356 
357  computeProba(Nbpoint);
358  computeMI(MI);
361  computeGradient();
362  GDirect = G;
363 
365  H = HDirect + HInverse;
367  }
368  G = GDirect - GInverse;
369 
370  try {
371  if (minimizationMethod == vpTemplateTrackerMIESM::USE_GRADIENT)
372  dp = -gain * 0.3 * G;
373  else {
374  switch (hessianComputation) {
376  dp = gain * HLMdesireInverse * G;
377  break;
379  if (HLM.cond() > HLMdesire.cond()) {
380  dp = gain * HLMdesireInverse * G;
381  } else {
382  dp = gain * 0.3 * HLM.inverseByLU() * G;
383  }
384  break;
385  default:
386  dp = gain * 0.3 * HLM.inverseByLU() * G;
387  break;
388  }
389  }
390  } catch (const vpException &e) {
391  throw(e);
392  }
393 
395  dp = -dp;
396 
397  if (useBrent) {
398  alpha = 2.;
399  computeOptimalBrentGain(I, p, -MI, dp, alpha);
400  dp = alpha * dp;
401  }
402  p += dp;
403 
404  iteration++;
405  }
406  } while ((iteration < iterationMax));
407 
408  MI_postEstimation = -getCost(I, p);
410  MI_postEstimation = -1;
411  }
412 
413  nbIteration = iteration;
414 }
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:142
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:303
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
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:247
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1590
unsigned int getHeight() const
Definition: vpImage.h:189
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6600
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6660
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.