Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  p_prec(), G_prec(), KQuasiNewton()
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) * static_cast<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)(static_cast<double>(-it) + et);
78 
79  for (unsigned int ip = 0; ip < nbParam; ++ip) {
80  ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)(static_cast<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)(static_cast<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  double Nc_255_ = (Nc - 1) / 255.;
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  ptTemplate[point].dW = new double[nbParam];
109 
110  double dx = ptTemplate[point].dx * Nc_255_;
111  double dy = ptTemplate[point].dy * Nc_255_;
112 
113  Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
114  double Tij = ptTemplate[point].val;
115  int ct = static_cast<int>(Tij * Nc_255_);
116  double et = (Tij * Nc_255_) - ct;
117  ptTemplateSupp[point].et = et;
118  ptTemplateSupp[point].ct = ct;
119  }
120  CompoInitialised = true;
121 }
122 
124 {
125  initCompInverse(I);
126 
127  int Nbpoint = 0;
128 
129  double IW;
130  int cr, ct;
131  double er, et;
132 
133  Nbpoint = 0;
134 
135  if (blur)
137 
139  Warp->computeCoeff(p);
140 
141  for (unsigned int point = 0; point < templateSize; point++) {
142  int i = ptTemplate[point].y;
143  int j = ptTemplate[point].x;
144  X1[0] = j;
145  X1[1] = i;
146 
147  Warp->computeDenom(X1, p);
148  Warp->warpX(X1, X2, p);
149 
150  double j2 = X2[0];
151  double i2 = X2[1];
152 
153  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
154  Nbpoint++;
155 
156  if (blur)
157  IW = BI.getValue(i2, j2);
158  else
159  IW = I.getValue(i2, j2);
160 
161  ct = ptTemplateSupp[point].ct;
162  et = ptTemplateSupp[point].et;
163  cr = static_cast<int>((IW * (Nc - 1)) / 255.);
164  er = (IW * (Nc - 1)) / 255. - cr;
165 
166  if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
167  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
168  bspline);
169  } else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
170  (ptTemplateSelect[point] || !useTemplateSelect)) {
171  if (bspline == 3) {
172  vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
173  } else {
174  vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
175  }
176  } else if (ptTemplateSelect[point] || !useTemplateSelect)
177  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
178  }
179  }
180 
181  double MI;
182  computeProba(Nbpoint);
183  computeMI(MI);
185 
186  lambda = lambdaDep;
187 
189 
191  KQuasiNewton = HLMdesireInverse;
192 }
193 
195 {
196  if (!CompoInitialised) {
197  std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
198  }
199 
200  dW = 0;
201 
202  if (blur)
204 
205  lambda = lambdaDep;
206  double MI = 0, MIprec = -1000;
207 
208  vpColVector p_avant_estimation;
209  p_avant_estimation = p;
210  MI_preEstimation = -getCost(I, p);
212 
213  initPosEvalRMS(p);
214 
215  vpColVector dpinv(nbParam);
216  double alpha = 2.;
217 
218  unsigned int iteration = 0;
219 
220  vpMatrix Hnorm(nbParam, nbParam);
221  double evolRMS_init = 0;
222  double evolRMS_prec = 0;
223  double evolRMS_delta;
224 
225  vpColVector dp_test_LMA(nbParam);
226  vpColVector dpinv_test_LMA(nbParam);
227  vpColVector p_test_LMA(nbParam);
228 
229  do {
230  int Nbpoint = 0;
231  MIprec = MI;
232  MI = 0;
233 
235 
236  Warp->computeCoeff(p);
237 
238  for (int point = 0; point < static_cast<int>(templateSize); point++) {
239  X1[0] = static_cast<double>(ptTemplate[point].x);
240  X1[1] = static_cast<double>(ptTemplate[point].y);
241 
242  Warp->computeDenom(X1, p);
243  Warp->warpX(X1, X2, p);
244 
245  double j2 = X2[0];
246  double i2 = X2[1];
247 
248  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
249 
250  Nbpoint++;
251  double IW;
252  if (!blur)
253  IW = static_cast<double>(I.getValue(i2, j2));
254  else
255  IW = BI.getValue(i2, j2);
256 
257  int ct = ptTemplateSupp[point].ct;
258  double et = ptTemplateSupp[point].et;
259  double tmp = IW * (static_cast<double>(Nc) - 1.) / 255.;
260  int cr = static_cast<int>(tmp);
261  double er = tmp - static_cast<double>(cr);
262 
264  (ptTemplateSelect[point] || !useTemplateSelect)) {
265  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
266  nbParam, bspline);
267  } else if (ptTemplateSelect[point] || !useTemplateSelect) {
268  if (bspline == 3) {
269  vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
270  ptTemplate[point].dW, nbParam);
271  } else {
272  vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
273  ptTemplate[point].dW, nbParam);
274  }
275  } else {
276  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
277  }
278  }
279  }
280 
281  if (Nbpoint == 0) {
282  diverge = true;
283  MI = 0;
284  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
285 
286  } else {
287  unsigned int indd, indd2;
288  indd = indd2 = 0;
289  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
290  for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
291  Prt[i] /= Nbpoint;
292  for (unsigned int j = 0; j < nbParam; j++) {
293  dPrt[indd] /= Nbpoint;
294  indd++;
295  for (unsigned int k = 0; k < nbParam; k++) {
296  d2Prt[indd2] /= Nbpoint;
297  indd2++;
298  }
299  }
300  }
301 
302  computeMI(MI);
303 
306  computeHessien(H);
307  }
308  computeGradient();
310 
311  try {
312  switch (hessianComputation) {
314  dp = gain * HLMdesireInverse * G;
315  break;
317  if (HLM.cond() > HLMdesire.cond())
318  dp = gain * HLMdesireInverse * G;
319  else
320  dp = gain * 0.2 * HLM.inverseByLU() * G;
321  break;
322  default:
323  dp = gain * 0.2 * HLM.inverseByLU() * G;
324  break;
325  }
326  } catch (const vpException &e) {
327  throw(e);
328  }
329  }
330 
331  switch (minimizationMethod) {
334  dp_test_LMA = -100000.1 * dp;
335  else
336  dp_test_LMA = dp;
337  Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
338  Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
339 
340  MI = -getCost(I, p);
341  double MI_LMA = -getCost(I, p_test_LMA);
342  if (MI_LMA > MI) {
343  dp = dp_test_LMA;
344  lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
345  } else {
346  dp = 0;
347  lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
348  }
349  } break;
351  dp = -gain * 0.3 * G * 20;
352  break;
353 
355  if (iterationGlobale != 0) {
356  vpColVector s_quasi = p - p_prec;
357  vpColVector y_quasi = G - G_prec;
358  double s_scal_y = s_quasi.t() * y_quasi;
359  if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
360  KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
361  KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
362  (y_quasi.t() * KQuasiNewton * y_quasi));
363  }
364  }
365  dp = gain * KQuasiNewton * G;
366  p_prec = p;
367  G_prec = G;
368  } break;
369 
370  default: {
371  if (useBrent) {
372  alpha = 2.;
373  computeOptimalBrentGain(I, p, -MI, dp, alpha);
374  dp = alpha * dp;
375  }
377  dp = - dp;
378 
379  break;
380  }
381  }
382 
383  Warp->getParamInverse(dp, dpinv);
384  Warp->pRondp(p, dpinv, p);
385  computeEvalRMS(p);
386 
387  if (iteration == 0) {
388  evolRMS_init = evolRMS;
389  }
390  iteration++;
392 
393  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
394  evolRMS_prec = evolRMS;
395 
396  } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) *std::numeric_limits<double>::epsilon()) &&
397  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init)*evolRMS_eps) );
398 
399  nbIteration = iteration;
400 
401  if (diverge) {
402  if (computeCovariance) {
404  covarianceMatrix = -1;
405  MI_postEstimation = -1;
406  NMI_postEstimation = -1;
407  }
408  } else {
409  MI_postEstimation = -getCost(I, p);
411 
413  p = p_avant_estimation;
417  covarianceMatrix = -1;
418  }
419 
420  if (computeCovariance) {
421  try {
422  covarianceMatrix = (-H).inverseByLU();
423  } catch (...) {
425  covarianceMatrix = -1;
426  MI_postEstimation = -1;
427  NMI_postEstimation = -1;
428  }
429  }
430  }
431 }
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void initTemplateRefBspline(unsigned int ptIndex, double &et)
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)
unsigned int getNbParam() const
vpMatrix inverseByLU() const
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
void initPosEvalRMS(const vpColVector &p)
vpRowVector t() const
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
void computeEvalRMS(const vpColVector &p)
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
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)
vpImage< double > dIx
void computeMI(double &MI)
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =0
vpImage< double > dIy
vpImage< double > d2Iy
unsigned int iterationGlobale
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
unsigned int nbIteration
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpImage< double > d2Ixy
void computeHessienNormalized(vpMatrix &H)
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