Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
vpTemplateTrackerMIInverseCompositional.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 #include <visp3/core/vpTrackingException.h>
41 #include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
42 
43 #include <memory>
44 
46  : vpTemplateTrackerMI(_warp), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
47  evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(1e-20), p_prec(), G_prec(), KQuasiNewton() //, useAYOptim(false)
48 {
49  useInverse = true;
50 }
51 
52 void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned int ptIndex, double &et) // AY : Optim
53 {
54  ptTemplateSupp[ptIndex].BtInit = new double[(1 + nbParam + nbParam * nbParam) * (unsigned int)bspline];
55 
56  unsigned int index = 0;
57  int endIndex = 1;
58 
59  double (*ptBspFct)(double);
60  double (*ptdBspFct)(double);
61  double (*ptd2BspFct)(double);
62  if (bspline == 3) {
63  if (et > 0.5) {
64  et = et - 1;
65  }
66  ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
67  ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
68  ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
69  } else {
70  ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
71  ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
72  ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
73  endIndex = 2;
74  }
75 
76  for (int it = -1; it <= endIndex; it++) {
77  ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it) + et);
78 
79  for (unsigned int ip = 0; ip < nbParam; ++ip) {
80  ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it) + et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
81  for (unsigned int ip2 = 0; ip2 < nbParam; ++ip2) {
82  ptTemplateSupp[ptIndex].BtInit[index++] =
83  (*ptd2BspFct)((double)(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
84  }
85  }
86  }
87 }
88 
90 {
91  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
92 
95 
101  }
102 
103  Warp->computeCoeff(p);
104  for (unsigned int point = 0; point < templateSize; point++) {
105  int i = ptTemplate[point].y;
106  int j = ptTemplate[point].x;
107 
108  X1[0] = j;
109  X1[1] = i;
110 
111  Warp->computeDenom(X1, p);
112  ptTemplate[point].dW = new double[nbParam];
113 
114  double dx = ptTemplate[point].dx * (Nc - 1) / 255.;
115  double dy = ptTemplate[point].dy * (Nc - 1) / 255.;
116 
117  Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
118  double Tij = ptTemplate[point].val;
119  int ct = (int)((Tij * (Nc - 1)) / 255.);
120  double et = (Tij * (Nc - 1)) / 255. - ct;
121 
122  ptTemplateSupp[point].et = et;
123  ptTemplateSupp[point].ct = ct;
124 
125  // ###### AY Optim
126  // if(useAYOptim)
127  // if(ApproxHessian != HESSIAN_NONSECOND /*&&
128  // hessianComputation !=
129  // vpTemplateTrackerMI::USE_HESSIEN_DESIRE*/)
130  // initTemplateRefBspline(point, et);
131  // ###################
132  }
133  CompoInitialised = true;
134 }
136 {
137  initCompInverse(I);
138 
139  // double erreur=0;
140  int Nbpoint = 0;
141 
142  // double Tij;
143  double IW;
144  int cr, ct;
145  double er, et;
146 
147  Nbpoint = 0;
148  // erreur=0;
149 
150  if (blur)
152 
154  Warp->computeCoeff(p);
155 
156  // AY : Optim
157  // unsigned int totParam = (bspline *
158  // bspline)*(1+nbParam+nbParam*nbParam); unsigned int size = (1 + nbParam
159  // + nbParam*nbParam)*bspline; double *ptb;
160 
161  for (unsigned int point = 0; point < templateSize; point++) {
162  int i = ptTemplate[point].y;
163  int j = ptTemplate[point].x;
164  X1[0] = j;
165  X1[1] = i;
166 
167  Warp->computeDenom(X1, p);
168  Warp->warpX(X1, X2, p);
169 
170  double j2 = X2[0];
171  double i2 = X2[1];
172 
173  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
174  Nbpoint++;
175  // Tij=ptTemplate[point].val;
176 
177  if (blur)
178  IW = BI.getValue(i2, j2);
179  else
180  IW = I.getValue(i2, j2);
181 
182  ct = ptTemplateSupp[point].ct;
183  et = ptTemplateSupp[point].et;
184  cr = (int)((IW * (Nc - 1)) / 255.);
185  er = ((double)IW * (Nc - 1)) / 255. - cr;
186 
187  // calcul de l'erreur
188  // erreur+=(Tij-IW)*(Tij-IW);
189 
190  if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
191  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
192  bspline);
193  } else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
194  (ptTemplateSelect[point] || !useTemplateSelect)) {
195  if (bspline == 3) {
196  vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
197  // {
198  // if(et>0.5){ct++;}
199  // if(er>0.5){cr++;}
200  // int index = (cr*Nc+ct)*totParam;
201  // double *ptb = &PrtTout[index];
202  // vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb,
203  // er, ptTemplateSupp[point].BtInit, size);
204  // }
205  } else {
206  vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
207 
208  // {
209  // // ################### AY : Optim
210  // unsigned int index = (cr*Nc+ct)*totParam;
211  // ptb = &PrtTout[index];
212  // vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb,
213  // er, ptTemplateSupp[point].BtInit, size);
214  // // ###################
215  // }
216  }
217  } else if (ptTemplateSelect[point] || !useTemplateSelect)
218  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
219  }
220  }
221 
222  double MI;
223  computeProba(Nbpoint);
224  computeMI(MI);
226 
227  lambda = lambdaDep;
228 
230 
232  KQuasiNewton = HLMdesireInverse;
233 }
234 
236 {
237  if (!CompoInitialised)
238  std::cout << "Compositionnal tracking no initialised\nUse "
239  "InitCompInverse(vpImage<unsigned char> &I) function"
240  << std::endl;
241  dW = 0;
242 
243  if (blur)
245 
246  lambda = lambdaDep;
247  double MI = 0, MIprec = -1000;
248 
249  vpColVector p_avant_estimation;
250  p_avant_estimation = p;
251  MI_preEstimation = -getCost(I, p);
253 
254  // std::cout << "MI avant: " << MI_preEstimation << std::endl;
255  // std::cout << "NMI avant: " << NMI_preEstimation << std::endl;
256 
257  initPosEvalRMS(p);
258 
259  vpColVector dpinv(nbParam);
260  double alpha = 2.;
261 
262  unsigned int iteration = 0;
263 
264  // unsigned int bspline_ = (unsigned int) bspline;
265  // unsigned int totParam = (bspline_ *
266  // bspline_)*(1+nbParam+nbParam*nbParam);
267 
268  vpMatrix Hnorm(nbParam, nbParam);
269 
270  do {
271  int Nbpoint = 0;
272  MIprec = MI;
273  MI = 0;
274 
276 
277  Warp->computeCoeff(p);
278 
279  {
280  for (int point = 0; point < (int)templateSize; point++) {
281  vpColVector x1(2), x2(2);
282  double i2, j2;
283 
284  x1[0] = (double)ptTemplate[point].x;
285  x1[1] = (double)ptTemplate[point].y;
286 
287  Warp->computeDenom(x1, p); // A modif pour parallelisation mais ne
288  // pose pas de pb avec warp utilises dans
289  // DECSA
290  Warp->warpX(x1, x2, p);
291 
292  j2 = x2[0];
293  i2 = x2[1];
294 
295  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
296  // if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() ==
297  // I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() &&
298  // (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
299  {
300  Nbpoint++;
301  double IW;
302  if (!blur)
303  IW = (double)I.getValue(i2, j2);
304  else
305  IW = BI.getValue(i2, j2);
306 
307  int ct = ptTemplateSupp[point].ct;
308  double et = ptTemplateSupp[point].et;
309  double tmp = IW * (((double)Nc) - 1.f) / 255.f;
310  int cr = (int)tmp;
311  double er = tmp - (double)cr;
312 
314  (ptTemplateSelect[point] || !useTemplateSelect)) {
315  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
316  nbParam, bspline);
317  } else if (ptTemplateSelect[point] || !useTemplateSelect) {
318  if (bspline == 3) {
319  vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
320  ptTemplate[point].dW, nbParam);
321  } else {
322  vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
323  ptTemplate[point].dW, nbParam);
324  }
325  } else {
326  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
327  }
328  }
329  }
330  }
331  }
332 
333  if (Nbpoint == 0) {
334  diverge = true;
335  MI = 0;
337  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
338 
339  } else {
340  // computeProba(Nbpoint);
341 
342  unsigned int indd, indd2;
343  indd = indd2 = 0;
344  unsigned int Ncb_ = (unsigned int)Ncb;
345  for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
346  Prt[i] = Prt[i] / Nbpoint;
347  for (unsigned int j = 0; j < nbParam; j++) {
348  dPrt[indd] = dPrt[indd] / Nbpoint;
349  indd++;
350  for (unsigned int k = 0; k < nbParam; k++) {
351  d2Prt[indd2] = d2Prt[indd2] / Nbpoint;
352  indd2++;
353  }
354  }
355  }
356 
357  computeMI(MI);
358 
361  computeHessien(H);
362  }
363  computeGradient();
364 
366 
367  try {
368  switch (hessianComputation) {
370  dp = gain * HLMdesireInverse * G;
371  break;
373  if (HLM.cond() > HLMdesire.cond())
374  dp = gain * HLMdesireInverse * G;
375  else
376  dp = gain * 0.2 * HLM.inverseByLU() * G;
377  break;
378  default:
379  dp = gain * 0.2 * HLM.inverseByLU() * G;
380  break;
381  }
382  } catch (const vpException &e) {
383  // std::cerr<<"probleme inversion"<<std::endl;
384  throw(e);
385  }
386  }
387 
388  switch (minimizationMethod) {
390  vpColVector dp_test_LMA(nbParam);
391  vpColVector dpinv_test_LMA(nbParam);
392  vpColVector p_test_LMA(nbParam);
394  dp_test_LMA = -100000.1 * dp;
395  else
396  dp_test_LMA = 1. * dp;
397  Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
398  Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
399 
400  MI = -getCost(I, p);
401  double MI_LMA = -getCost(I, p_test_LMA);
402  if (MI_LMA > MI) {
403  dp = dp_test_LMA;
404  lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
405  } else {
406  dp = 0;
407  lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
408  }
409  } break;
411  dp = -gain * 0.3 * G * 20;
412  break;
413 
415  if (iterationGlobale != 0) {
416  vpColVector s_quasi = p - p_prec;
417  vpColVector y_quasi = G - G_prec;
418  double s_scal_y = s_quasi.t() * y_quasi;
419  // std::cout<<"mise a jour K"<<std::endl;
420  /*if(s_scal_y!=0)//BFGS
421  KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
422  // if(s_scal_y!=0)//DFP
423  if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) // DFP
424  {
425  KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
426  KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
427  (y_quasi.t() * KQuasiNewton * y_quasi));
428  // std::cout<<"mise a jour K"<<std::endl;
429  }
430  }
431  dp = gain * KQuasiNewton * G;
432  // std::cout<<KQuasiNewton<<std::endl<<std::endl;
433  p_prec = p;
434  G_prec = G;
435  // p-=1.01*dp;
436  } break;
437 
438  default: {
439  if (useBrent) {
440  alpha = 2.;
441  computeOptimalBrentGain(I, p, -MI, dp, alpha);
442  dp = alpha * dp;
443  }
445  dp = -1. * dp;
446 
447  break;
448  }
449  }
450 
451  Warp->getParamInverse(dp, dpinv);
452  Warp->pRondp(p, dpinv, p);
453 
454  iteration++;
456 
457  computeEvalRMS(p);
458 
459  // std::cout << p.t() << std::endl;
460  } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
461  (iteration < iterationMax) && (evolRMS > threshold_RMS));
462  // while( (!diverge) && (MI!=MIprec) &&(iteration<
463  // iterationMax)&&(evolRMS>threshold_RMS) );
464 
465  nbIteration = iteration;
466 
467  if (diverge) {
468  if (computeCovariance) {
470  covarianceMatrix = -1;
471  MI_postEstimation = -1;
472  NMI_postEstimation = -1;
473  }
475 
476  // throw(vpTrackingException(vpTrackingException::badValue,
477  // "Tracking failed")) ;
478  } else {
479  MI_postEstimation = -getCost(I, p);
481  // std::cout << "MI apres: " << MI_postEstimation << std::endl;
482  // std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
484  p = p_avant_estimation;
488  covarianceMatrix = -1;
489  }
490 
492 
493  if (computeCovariance) {
494  try {
495  covarianceMatrix = (-H).inverseByLU();
496  // covarianceMatrix = (-Hnorm).inverseByLU();
497  } catch (...) {
499  covarianceMatrix = -1;
500  MI_postEstimation = -1;
501  NMI_postEstimation = -1;
503  }
504  }
505  }
506 }
507 
509 {
510  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
511  x_pos = new double[nb_corners];
512  y_pos = new double[nb_corners];
513 
514  Warp->computeCoeff(pw);
515  vpTemplateTrackerTriangle triangle;
516 
517  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
518  zoneTracked->getTriangle(i, triangle);
519  for (unsigned int j = 0; j < 3; j++) {
520  triangle.getCorner(j, X1[0], X1[1]);
521 
522  Warp->computeDenom(X1, pw);
523  Warp->warpX(X1, X2, p);
524  x_pos[i * 3 + j] = X2[0];
525  y_pos[i * 3 + j] = X2[1];
526  }
527  }
528 }
529 
531 {
532  unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
533 
534  Warp->computeCoeff(pw);
535  evolRMS = 0;
536  vpTemplateTrackerTriangle triangle;
537 
538  for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
539  zoneTracked->getTriangle(i, triangle);
540  for (unsigned int j = 0; j < 3; j++) {
541  triangle.getCorner(j, X1[0], X1[1]);
542 
543  Warp->computeDenom(X1, pw);
544  Warp->warpX(X1, X2, pw);
545  evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
546  (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
547  x_pos[i * 3 + j] = X2[0];
548  y_pos[i * 3 + j] = X2[1];
549  }
550  }
551  evolRMS = evolRMS / nb_corners;
552 }
553 
555 {
556  delete[] x_pos;
557  delete[] y_pos;
558 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void initTemplateRefBspline(unsigned int ptIndex, double &et)
void computeHessien(vpMatrix &H)
void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
unsigned int getWidth() const
Definition: vpImage.h:244
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpColVector getCorner(unsigned int i) const
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
virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const =0
unsigned int templateSize
unsigned int iterationMax
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1422
Error that can be emited by the vpTracker class and its derivates.
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
unsigned int getNbTriangle() const
vpImage< double > dIx
void computeMI(double &MI)
vpRowVector t() const
vpImage< double > dIy
vpImage< double > d2Iy
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5133
unsigned int iterationGlobale
unsigned int getNbParam() const
virtual void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)=0
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
unsigned int nbIteration
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpImage< double > d2Ixy
vpMatrix inverseByLU() const
vpTemplateTrackerZone * zoneTracked
unsigned int getHeight() const
Definition: vpImage.h:186
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
void computeHessienNormalized(vpMatrix &H)
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5193
void computeProba(int &nbpoint)
vpHessienType hessianComputation