Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpTemplateTrackerMIInverseCompositional.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
38 *****************************************************************************/
39 #include <visp3/core/vpTrackingException.h>
40 #include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
41 
42 #include <memory>
43 
44 BEGIN_VISP_NAMESPACE
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  }
70  else {
71  ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
72  ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
73  ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
74  endIndex = 2;
75  }
76 
77  for (int it = -1; it <= endIndex; it++) {
78  ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)(static_cast<double>(-it) + et);
79 
80  for (unsigned int ip = 0; ip < nbParam; ++ip) {
81  ptTemplateSupp[ptIndex].BtInit[index++] =
82  (*ptdBspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
83  for (unsigned int ip2 = 0; ip2 < nbParam; ++ip2) {
84  ptTemplateSupp[ptIndex].BtInit[index++] =
85  (*ptd2BspFct)(static_cast<double>(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
86  }
87  }
88  }
89 }
90 
92 {
93  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
94 
97 
103  }
104  double Nc_255_ = (Nc - 1) / 255.;
105  Warp->computeCoeff(p);
106  for (unsigned int point = 0; point < templateSize; point++) {
107  int i = ptTemplate[point].y;
108  int j = ptTemplate[point].x;
109 
110  ptTemplate[point].dW = new double[nbParam];
111 
112  double dx = ptTemplate[point].dx * Nc_255_;
113  double dy = ptTemplate[point].dy * Nc_255_;
114 
115  Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
116  double Tij = ptTemplate[point].val;
117  int ct = static_cast<int>(Tij * Nc_255_);
118  double et = (Tij * Nc_255_) - ct;
119  ptTemplateSupp[point].et = et;
120  ptTemplateSupp[point].ct = ct;
121  }
122  CompoInitialised = true;
123 }
124 
126 {
127  initCompInverse(I);
128 
129  int Nbpoint = 0;
130 
131  double IW;
132  int cr, ct;
133  double er, et;
134 
135  Nbpoint = 0;
136 
137  if (blur)
139 
141  Warp->computeCoeff(p);
142 
143  for (unsigned int point = 0; point < templateSize; point++) {
144  int i = ptTemplate[point].y;
145  int j = ptTemplate[point].x;
146  X1[0] = j;
147  X1[1] = i;
148 
149  Warp->computeDenom(X1, p);
150  Warp->warpX(X1, X2, p);
151 
152  double j2 = X2[0];
153  double i2 = X2[1];
154 
155  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
156  Nbpoint++;
157 
158  if (blur)
159  IW = BI.getValue(i2, j2);
160  else
161  IW = I.getValue(i2, j2);
162 
163  ct = ptTemplateSupp[point].ct;
164  et = ptTemplateSupp[point].et;
165  cr = static_cast<int>((IW * (Nc - 1)) / 255.);
166  er = (IW * (Nc - 1)) / 255. - cr;
167 
168  if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
169  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
170  bspline);
171  }
172  else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
173  (ptTemplateSelect[point] || !useTemplateSelect)) {
174  if (bspline == 3) {
175  vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
176  }
177  else {
178  vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
179  }
180  }
181  else if (ptTemplateSelect[point] || !useTemplateSelect)
182  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
183  }
184  }
185 
186  double MI;
187  computeProba(Nbpoint);
188  computeMI(MI);
190 
191  lambda = lambdaDep;
192 
194 
196  KQuasiNewton = HLMdesireInverse;
197 }
198 
200 {
201  if (!CompoInitialised) {
202  std::cout << "Compositionnal tracking not initialised.\nUse initCompInverse() function." << std::endl;
203  }
204 
205  dW = 0;
206 
207  if (blur)
209 
210  lambda = lambdaDep;
211  double MI = 0, MIprec = -1000;
212 
213  vpColVector p_avant_estimation;
214  p_avant_estimation = p;
215  MI_preEstimation = -getCost(I, p);
217 
218  initPosEvalRMS(p);
219 
220  vpColVector dpinv(nbParam);
221  double alpha = 2.;
222 
223  unsigned int iteration = 0;
224 
225  vpMatrix Hnorm(nbParam, nbParam);
226  double evolRMS_init = 0;
227  double evolRMS_prec = 0;
228  double evolRMS_delta;
229 
230  vpColVector dp_test_LMA(nbParam);
231  vpColVector dpinv_test_LMA(nbParam);
232  vpColVector p_test_LMA(nbParam);
233 
234  do {
235  int Nbpoint = 0;
236  MIprec = MI;
237  MI = 0;
238 
240 
241  Warp->computeCoeff(p);
242 
243  for (int point = 0; point < static_cast<int>(templateSize); point++) {
244  X1[0] = static_cast<double>(ptTemplate[point].x);
245  X1[1] = static_cast<double>(ptTemplate[point].y);
246 
247  Warp->computeDenom(X1, p);
248  Warp->warpX(X1, X2, p);
249 
250  double j2 = X2[0];
251  double i2 = X2[1];
252 
253  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
254 
255  Nbpoint++;
256  double IW;
257  if (!blur)
258  IW = static_cast<double>(I.getValue(i2, j2));
259  else
260  IW = BI.getValue(i2, j2);
261 
262  int ct = ptTemplateSupp[point].ct;
263  double et = ptTemplateSupp[point].et;
264  double tmp = IW * (static_cast<double>(Nc) - 1.) / 255.;
265  int cr = static_cast<int>(tmp);
266  double er = tmp - static_cast<double>(cr);
267 
269  (ptTemplateSelect[point] || !useTemplateSelect)) {
270  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
271  nbParam, bspline);
272  }
273  else if (ptTemplateSelect[point] || !useTemplateSelect) {
274  if (bspline == 3) {
275  vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
276  nbParam);
277  }
278  else {
279  vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
280  nbParam);
281  }
282  }
283  else {
284  vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
285  }
286  }
287  }
288 
289  if (Nbpoint == 0) {
290  diverge = true;
291  MI = 0;
292  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
293 
294  }
295  else {
296  unsigned int indd, indd2;
297  indd = indd2 = 0;
298  unsigned int Ncb_ = static_cast<unsigned int>(Ncb);
299  for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
300  Prt[i] /= Nbpoint;
301  for (unsigned int j = 0; j < nbParam; j++) {
302  dPrt[indd] /= Nbpoint;
303  indd++;
304  for (unsigned int k = 0; k < nbParam; k++) {
305  d2Prt[indd2] /= Nbpoint;
306  indd2++;
307  }
308  }
309  }
310 
311  computeMI(MI);
312 
315  computeHessien(H);
316  }
317  computeGradient();
319 
320  try {
321  switch (hessianComputation) {
323  dp = gain * HLMdesireInverse * G;
324  break;
326  if (HLM.cond() > HLMdesire.cond())
327  dp = gain * HLMdesireInverse * G;
328  else
329  dp = gain * 0.2 * HLM.inverseByLU() * G;
330  break;
331  default:
332  dp = gain * 0.2 * HLM.inverseByLU() * G;
333  break;
334  }
335  }
336  catch (const vpException &e) {
337  throw(e);
338  }
339  }
340 
341  switch (minimizationMethod) {
344  dp_test_LMA = -100000.1 * dp;
345  else
346  dp_test_LMA = dp;
347  Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
348  Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
349 
350  MI = -getCost(I, p);
351  double MI_LMA = -getCost(I, p_test_LMA);
352  if (MI_LMA > MI) {
353  dp = dp_test_LMA;
354  lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
355  }
356  else {
357  dp = 0;
358  lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
359  }
360  } break;
362  dp = -gain * 0.3 * G * 20;
363  break;
364 
366  if (iterationGlobale != 0) {
367  vpColVector s_quasi = p - p_prec;
368  vpColVector y_quasi = G - G_prec;
369  double s_scal_y = s_quasi.t() * y_quasi;
370  if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) {
371  KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
372  KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
373  (y_quasi.t() * KQuasiNewton * y_quasi));
374  }
375  }
376  dp = gain * KQuasiNewton * G;
377  p_prec = p;
378  G_prec = G;
379  } break;
380 
381  default: {
382  if (useBrent) {
383  alpha = 2.;
384  computeOptimalBrentGain(I, p, -MI, dp, alpha);
385  dp = alpha * dp;
386  }
388  dp = -dp;
389 
390  break;
391  }
392  }
393 
394  Warp->getParamInverse(dp, dpinv);
395  Warp->pRondp(p, dpinv, p);
396  computeEvalRMS(p);
397 
398  if (iteration == 0) {
399  evolRMS_init = evolRMS;
400  }
401  iteration++;
403 
404  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
405  evolRMS_prec = evolRMS;
406 
407  } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
408  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
409 
410  nbIteration = iteration;
411 
412  if (diverge) {
413  if (computeCovariance) {
415  covarianceMatrix = -1;
416  MI_postEstimation = -1;
417  NMI_postEstimation = -1;
418  }
419  }
420  else {
421  MI_postEstimation = -getCost(I, p);
423 
425  p = p_avant_estimation;
429  covarianceMatrix = -1;
430  }
431 
432  if (computeCovariance) {
433  try {
434  covarianceMatrix = (-H).inverseByLU();
435  }
436  catch (...) {
438  covarianceMatrix = -1;
439  MI_postEstimation = -1;
440  NMI_postEstimation = -1;
441  }
442  }
443  }
444 }
445 END_VISP_NAMESPACE
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition: vpException.h:60
static void getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx, const vpImage< bool > *p_mask=nullptr)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false, const vpImage< bool > *p_mask=nullptr)
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy, const vpImage< bool > *p_mask=nullptr)
unsigned int getWidth() const
Definition: vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
unsigned int getHeight() const
Definition: vpImage.h:181
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1806
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:1869
void initTemplateRefBspline(unsigned int ptIndex, double &et)
vpHessienApproximationType ApproxHessian
vpImage< double > d2Ix
void computeHessienNormalized(vpMatrix &H)
vpHessienType hessianComputation
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) VP_OVERRIDE
void computeMI(double &MI)
void computeProba(int &nbpoint)
void computeHessien(vpMatrix &H)
vpImage< double > d2Ixy
double getNormalizedCost(const vpImage< unsigned char > &I, const vpColVector &tp)
vpImage< double > d2Iy
unsigned int getNbParam() const
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =0
vpImage< double > dIx
vpImage< double > dIy
void computeEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int iterationGlobale
vpImage< double > BI
unsigned int templateSize
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.