Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 
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, 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  }
300  computeGradient();
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);
357  computeGradient();
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) {
372  dp = gain * HLMdesireInverse * G;
373  break;
375  if (HLM.cond() > HLMdesire.cond()) {
376  dp = gain * HLMdesireInverse * G;
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 
405  MI_postEstimation = -getCost(I, p);
407  MI_postEstimation = -1;
408  }
409 
410  nbIteration = iteration;
411 }
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
void computeHessien(vpMatrix &H)
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
vpTemplateTrackerMIESM()
Default constructor.
vpMatrix inverseByLU() const
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, const double *dwdp0, vpMatrix &dM)=0
error that can be emited by ViSP classes.
Definition: vpException.h:71
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpImage< double > d2Ix
vpImage< double > BI
vpHessienApproximationType ApproxHessian
unsigned int templateSize
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6622
virtual void getdWdp0(const int &v, const int &u, double *dIdW)=0
unsigned int iterationMax
Error that can be emited by the vpTracker class and its derivates.
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
void initHessienDesired(const vpImage< unsigned char > &I)
vpImage< double > dIx
void computeMI(double &MI)
vpImage< double > dIy
vpImage< double > d2Iy
virtual bool isESMcompatible() const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
unsigned int nbIteration
void trackNoPyr(const vpImage< unsigned char > &I)
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
vpMinimizationTypeMIESM minimizationMethod
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
unsigned int getHeight() const
Definition: vpImage.h:188
vpTemplateTrackerPointCompo * ptTemplateCompo
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpImage< double > d2Ixy
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
unsigned int getWidth() const
Definition: vpImage.h:246
void computeProba(int &nbpoint)
vpHessienType hessianComputation