Visual Servoing Platform  version 3.5.1 under development (2022-05-28)
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++] =
81  (*ptdBspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
82  for (unsigned int ip2 = 0; ip2 < nbParam; ++ip2) {
83  ptTemplateSupp[ptIndex].BtInit[index++] =
84  (*ptd2BspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
85  }
86  }
87  }
88 }
89 
91 {
92  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
93 
96 
102  }
103  double Nc_255_ = (Nc - 1) / 255.;
104  Warp->computeCoeff(p);
105  for (unsigned int point = 0; point < templateSize; point++) {
106  int i = ptTemplate[point].y;
107  int j = ptTemplate[point].x;
108 
109  ptTemplate[point].dW = new double[nbParam];
110 
111  double dx = ptTemplate[point].dx * Nc_255_;
112  double dy = ptTemplate[point].dy * Nc_255_;
113 
114  Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
115  double Tij = ptTemplate[point].val;
116  int ct = static_cast<int>(Tij * Nc_255_);
117  double et = (Tij * Nc_255_) - ct;
118  ptTemplateSupp[point].et = et;
119  ptTemplateSupp[point].ct = ct;
120  }
121  CompoInitialised = true;
122 }
123 
125 {
126  initCompInverse(I);
127 
128  int Nbpoint = 0;
129 
130  double IW;
131  int cr, ct;
132  double er, et;
133 
134  Nbpoint = 0;
135 
136  if (blur)
138 
140  Warp->computeCoeff(p);
141 
142  for (unsigned int point = 0; point < templateSize; point++) {
143  int i = ptTemplate[point].y;
144  int j = ptTemplate[point].x;
145  X1[0] = j;
146  X1[1] = i;
147 
148  Warp->computeDenom(X1, p);
149  Warp->warpX(X1, X2, p);
150 
151  double j2 = X2[0];
152  double i2 = X2[1];
153 
154  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
155  Nbpoint++;
156 
157  if (blur)
158  IW = BI.getValue(i2, j2);
159  else
160  IW = I.getValue(i2, j2);
161 
162  ct = ptTemplateSupp[point].ct;
163  et = ptTemplateSupp[point].et;
164  cr = static_cast<int>((IW * (Nc - 1)) / 255.);
165  er = (IW * (Nc - 1)) / 255. - cr;
166 
167  if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
168  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
169  bspline);
170  } else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
171  (ptTemplateSelect[point] || !useTemplateSelect)) {
172  if (bspline == 3) {
173  vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
174  } else {
175  vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
176  }
177  } else if (ptTemplateSelect[point] || !useTemplateSelect)
178  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
179  }
180  }
181 
182  double MI;
183  computeProba(Nbpoint);
184  computeMI(MI);
186 
187  lambda = lambdaDep;
188 
190 
192  KQuasiNewton = HLMdesireInverse;
193 }
194 
196 {
197  if (!CompoInitialised) {
198  std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
199  }
200 
201  dW = 0;
202 
203  if (blur)
205 
206  lambda = lambdaDep;
207  double MI = 0, MIprec = -1000;
208 
209  vpColVector p_avant_estimation;
210  p_avant_estimation = p;
211  MI_preEstimation = -getCost(I, p);
213 
214  initPosEvalRMS(p);
215 
216  vpColVector dpinv(nbParam);
217  double alpha = 2.;
218 
219  unsigned int iteration = 0;
220 
221  vpMatrix Hnorm(nbParam, nbParam);
222  double evolRMS_init = 0;
223  double evolRMS_prec = 0;
224  double evolRMS_delta;
225 
226  vpColVector dp_test_LMA(nbParam);
227  vpColVector dpinv_test_LMA(nbParam);
228  vpColVector p_test_LMA(nbParam);
229 
230  do {
231  int Nbpoint = 0;
232  MIprec = MI;
233  MI = 0;
234 
236 
237  Warp->computeCoeff(p);
238 
239  for (int point = 0; point < static_cast<int>(templateSize); point++) {
240  X1[0] = static_cast<double>(ptTemplate[point].x);
241  X1[1] = static_cast<double>(ptTemplate[point].y);
242 
243  Warp->computeDenom(X1, p);
244  Warp->warpX(X1, X2, p);
245 
246  double j2 = X2[0];
247  double i2 = X2[1];
248 
249  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
250 
251  Nbpoint++;
252  double IW;
253  if (!blur)
254  IW = static_cast<double>(I.getValue(i2, j2));
255  else
256  IW = BI.getValue(i2, j2);
257 
258  int ct = ptTemplateSupp[point].ct;
259  double et = ptTemplateSupp[point].et;
260  double tmp = IW * (static_cast<double>(Nc) - 1.) / 255.;
261  int cr = static_cast<int>(tmp);
262  double er = tmp - static_cast<double>(cr);
263 
265  (ptTemplateSelect[point] || !useTemplateSelect)) {
266  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
267  nbParam, bspline);
268  } else if (ptTemplateSelect[point] || !useTemplateSelect) {
269  if (bspline == 3) {
270  vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
271  nbParam);
272  } else {
273  vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
274  nbParam);
275  }
276  } else {
277  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
278  }
279  }
280  }
281 
282  if (Nbpoint == 0) {
283  diverge = true;
284  MI = 0;
285  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
286 
287  } else {
288  unsigned int indd, indd2;
289  indd = indd2 = 0;
290  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
291  for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
292  Prt[i] /= Nbpoint;
293  for (unsigned int j = 0; j < nbParam; j++) {
294  dPrt[indd] /= Nbpoint;
295  indd++;
296  for (unsigned int k = 0; k < nbParam; k++) {
297  d2Prt[indd2] /= Nbpoint;
298  indd2++;
299  }
300  }
301  }
302 
303  computeMI(MI);
304 
307  computeHessien(H);
308  }
309  computeGradient();
311 
312  try {
313  switch (hessianComputation) {
315  dp = gain * HLMdesireInverse * G;
316  break;
318  if (HLM.cond() > HLMdesire.cond())
319  dp = gain * HLMdesireInverse * G;
320  else
321  dp = gain * 0.2 * HLM.inverseByLU() * G;
322  break;
323  default:
324  dp = gain * 0.2 * HLM.inverseByLU() * G;
325  break;
326  }
327  } catch (const vpException &e) {
328  throw(e);
329  }
330  }
331 
332  switch (minimizationMethod) {
335  dp_test_LMA = -100000.1 * dp;
336  else
337  dp_test_LMA = dp;
338  Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
339  Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
340 
341  MI = -getCost(I, p);
342  double MI_LMA = -getCost(I, p_test_LMA);
343  if (MI_LMA > MI) {
344  dp = dp_test_LMA;
345  lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
346  } else {
347  dp = 0;
348  lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
349  }
350  } break;
352  dp = -gain * 0.3 * G * 20;
353  break;
354 
356  if (iterationGlobale != 0) {
357  vpColVector s_quasi = p - p_prec;
358  vpColVector y_quasi = G - G_prec;
359  double s_scal_y = s_quasi.t() * y_quasi;
360  if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
361  KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
362  KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
363  (y_quasi.t() * KQuasiNewton * y_quasi));
364  }
365  }
366  dp = gain * KQuasiNewton * G;
367  p_prec = p;
368  G_prec = G;
369  } break;
370 
371  default: {
372  if (useBrent) {
373  alpha = 2.;
374  computeOptimalBrentGain(I, p, -MI, dp, alpha);
375  dp = alpha * dp;
376  }
378  dp = -dp;
379 
380  break;
381  }
382  }
383 
384  Warp->getParamInverse(dp, dpinv);
385  Warp->pRondp(p, dpinv, p);
386  computeEvalRMS(p);
387 
388  if (iteration == 0) {
389  evolRMS_init = evolRMS;
390  }
391  iteration++;
393 
394  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
395  evolRMS_prec = evolRMS;
396 
397  } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
398  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
399 
400  nbIteration = iteration;
401 
402  if (diverge) {
403  if (computeCovariance) {
405  covarianceMatrix = -1;
406  MI_postEstimation = -1;
407  NMI_postEstimation = -1;
408  }
409  } else {
410  MI_postEstimation = -getCost(I, p);
412 
414  p = p_avant_estimation;
418  covarianceMatrix = -1;
419  }
420 
421  if (computeCovariance) {
422  try {
423  covarianceMatrix = (-H).inverseByLU();
424  } catch (...) {
426  covarianceMatrix = -1;
427  MI_postEstimation = -1;
428  NMI_postEstimation = -1;
429  }
430  }
431  }
432 }
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:1343
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:6610
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:6670
unsigned int getWidth() const
Definition: vpImage.h:246
void computeProba(int &nbpoint)
vpHessienType hessianComputation