Visual Servoing Platform  version 3.6.1 under development (2024-02-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 
42  : vpTemplateTrackerMI(_warp), CompoInitialised(false)
43 {
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  } else {
232  computeProba(Nbpoint);
233  computeMI(MI);
235  computeHessien(H);
236  computeGradient();
237 
239 
240  try {
241  switch (hessianComputation) {
243  dp = gain * HLMdesireInverse * G;
244  break;
246  if (HLM.cond() > HLMdesire.cond())
247  dp = gain * HLMdesireInverse * G;
248  else
249  dp = gain * 0.2 * HLM.inverseByLU() * G;
250  break;
251  default:
252  dp = gain * 0.2 * HLM.inverseByLU() * G;
253  break;
254  }
255  } catch (const vpException &e) {
256  throw(e);
257  }
258  }
259 
261  dp = -0.04 * dp;
262  // else
263  // dp = 1. * dp;
264 
265  if (useBrent) {
266  alpha = 2.;
267  computeOptimalBrentGain(I, p, -MI, dp, alpha);
268  dp = alpha * dp;
269  }
270  Warp->pRondp(p, dp, p);
271  computeEvalRMS(p);
272 
273  if (iteration == 0) {
274  evolRMS_init = evolRMS;
275  }
276 
277  iteration++;
278 
279  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
280  evolRMS_prec = evolRMS;
281 
282  } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
283  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
284 
285  nbIteration = iteration;
286 
287  MI_postEstimation = -getCost(I, p);
289  MI_postEstimation = -1;
290  }
291 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
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
Definition: vpImage.h:1567
unsigned int getHeight() const
Definition: vpImage.h:184
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6610
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6671
void initHessienDesired(const vpImage< unsigned char > &I)
vpHessienApproximationType ApproxHessian
vpHessienType hessianComputation
void computeMI(double &MI)
void computeProba(int &nbpoint)
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp) vp_override
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.