Visual Servoing Platform  version 3.6.1 under development (2024-10-13)
vpTemplateTrackerMIForwardCompositional.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/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
40 
41 BEGIN_VISP_NAMESPACE
43  : vpTemplateTrackerMI(_warp), CompoInitialised(false)
44 { }
45 
47 {
48  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
49  for (unsigned int point = 0; point < templateSize; point++) {
50  int i = ptTemplate[point].y;
51  int j = ptTemplate[point].x;
52  X1[0] = j;
53  X1[1] = i;
54  Warp->computeDenom(X1, p);
55  ptTemplate[point].dW = new double[2 * nbParam];
56  Warp->getdWdp0(i, j, ptTemplate[point].dW);
57 
58  double Tij = ptTemplate[point].val;
59  int ct = (int)((Tij * (Nc - 1)) / 255.);
60  double et = (Tij * (Nc - 1)) / 255. - ct;
61  ptTemplateSupp[point].et = et;
62  ptTemplateSupp[point].ct = ct;
63  ptTemplateSupp[point].Bt = new double[4];
64  ptTemplateSupp[point].dBt = new double[4];
65  for (char it = -1; it <= 2; it++) {
66  ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
67  ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
68  }
69  }
70  CompoInitialised = true;
71 }
73 {
74  initCompo();
75 
76  dW = 0;
77 
78  if (blur)
82 
83  int Nbpoint = 0;
84 
85  double IW, dx, dy;
86  int cr, ct;
87  double er, et;
88 
89  Nbpoint = 0;
90 
92 
93  Warp->computeCoeff(p);
94  for (unsigned int point = 0; point < templateSize; point++) {
95  int i = ptTemplate[point].y;
96  int j = ptTemplate[point].x;
97  X1[0] = j;
98  X1[1] = i;
99 
100  Warp->computeDenom(X1, p);
101  Warp->warpX(X1, X2, p);
102 
103  double j2 = X2[0];
104  double i2 = X2[1];
105 
106  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
107  Nbpoint++;
108  if (!blur)
109  IW = I.getValue(i2, j2);
110  else
111  IW = BI.getValue(i2, j2);
112 
113  dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
114  dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
115 
116  cr = ptTemplateSupp[point].ct;
117  er = ptTemplateSupp[point].et;
118  ct = (int)((IW * (Nc - 1)) / 255.);
119  et = ((double)IW * (Nc - 1)) / 255. - ct;
120 
121  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
122 
123  double *tptemp = new double[nbParam];
124  for (unsigned int it = 0; it < nbParam; it++)
125  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
126 
127  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
128 
129  delete[] tptemp;
130  }
131  }
132  double MI;
133  computeProba(Nbpoint);
134  computeMI(MI);
136 
137  lambda = lambdaDep;
138 
141 }
142 
144 {
145  if (!CompoInitialised) {
146  std::cout << "Compositionnal tracking not initialised.\nUse initCompo() function." << std::endl;
147  }
148  dW = 0;
149 
150  if (blur) {
152  }
155 
156  lambda = lambdaDep;
157  double MI = 0, MIprec = -1000;
158 
159  MI_preEstimation = -getCost(I, p);
160 
161  initPosEvalRMS(p);
162 
163  double i2, j2;
164  double IW;
165  int cr, ct;
166  double er, et;
167  double dx, dy;
168 
169  vpColVector dpinv(nbParam);
170  double alpha = 2.;
171 
172  int i, j;
173  unsigned int iteration = 0;
174 
175  double evolRMS_init = 0;
176  double evolRMS_prec = 0;
177  double evolRMS_delta;
178 
179  do {
180  int Nbpoint = 0;
181  MIprec = MI;
182  MI = 0;
183 
185 
186  Warp->computeCoeff(p);
187 
188  for (unsigned int point = 0; point < templateSize; point++) {
189  i = ptTemplate[point].y;
190  j = ptTemplate[point].x;
191  X1[0] = j;
192  X1[1] = i;
193  Warp->warpX(i, j, i2, j2, p);
194  X2[0] = j2;
195  X2[1] = i2;
196 
197  Warp->computeDenom(X1, p);
198  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
199  Nbpoint++;
200  if (!blur)
201  IW = I.getValue(i2, j2);
202  else
203  IW = BI.getValue(i2, j2);
204 
205  dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
206  dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
207 
208  ct = (int)((IW * (Nc - 1)) / 255.);
209  et = ((double)IW * (Nc - 1)) / 255. - ct;
210  cr = ptTemplateSupp[point].ct;
211  er = ptTemplateSupp[point].et;
212 
213  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
214 
215  double *tptemp = new double[nbParam];
216  for (unsigned int it = 0; it < nbParam; it++)
217  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
218 
220  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
222  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
223 
224  delete[] tptemp;
225  }
226  }
227  if (Nbpoint == 0) {
228  diverge = true;
229  MI = 0;
230  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
231  }
232  else {
233  computeProba(Nbpoint);
234  computeMI(MI);
236  computeHessien(H);
237  computeGradient();
238 
240 
241  try {
242  switch (hessianComputation) {
244  dp = gain * HLMdesireInverse * G;
245  break;
247  if (HLM.cond() > HLMdesire.cond())
248  dp = gain * HLMdesireInverse * G;
249  else
250  dp = gain * 0.2 * HLM.inverseByLU() * G;
251  break;
252  default:
253  dp = gain * 0.2 * HLM.inverseByLU() * G;
254  break;
255  }
256  }
257  catch (const vpException &e) {
258  throw(e);
259  }
260  }
261 
263  dp = -0.04 * dp;
264  // else
265  // dp = 1. * dp;
266 
267  if (useBrent) {
268  alpha = 2.;
269  computeOptimalBrentGain(I, p, -MI, dp, alpha);
270  dp = alpha * dp;
271  }
272  Warp->pRondp(p, dp, p);
273  computeEvalRMS(p);
274 
275  if (iteration == 0) {
276  evolRMS_init = evolRMS;
277  }
278 
279  iteration++;
280 
281  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
282  evolRMS_prec = evolRMS;
283 
284  } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
285  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
286 
287  nbIteration = iteration;
288 
289  MI_postEstimation = -getCost(I, p);
291  MI_postEstimation = -1;
292  }
293 }
294 END_VISP_NAMESPACE
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
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)
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
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1810
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:1873
void initHessienDesired(const vpImage< unsigned char > &I)
VP_EXPLICIT vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp)
vpHessienApproximationType ApproxHessian
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)
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, const double *dwdp0, vpMatrix &dM)=0
virtual void getdWdp0(const int &v, const int &u, 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
vpImage< double > BI
unsigned int templateSize
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.