Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpTemplateTrackerMIForwardCompositional.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/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
41 
43  : vpTemplateTrackerMI(_warp), CompoInitialised(false)
44 {
45 }
46 
48 {
49  std::cout << "Initialise precomputed value of Compositionnal Direct" << std::endl;
50  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
51  for (unsigned int point = 0; point < templateSize; point++) {
52  int i = ptTemplate[point].y;
53  int j = ptTemplate[point].x;
54  X1[0] = j;
55  X1[1] = i;
56  Warp->computeDenom(X1, p);
57  ptTemplate[point].dW = new double[2 * nbParam];
58  Warp->getdWdp0(i, j, ptTemplate[point].dW);
59 
60  double Tij = ptTemplate[point].val;
61  int ct = (int)((Tij * (Nc - 1)) / 255.);
62  double et = (Tij * (Nc - 1)) / 255. - ct;
63  ptTemplateSupp[point].et = et;
64  ptTemplateSupp[point].ct = ct;
65  ptTemplateSupp[point].Bt = new double[4];
66  ptTemplateSupp[point].dBt = new double[4];
67  for (char it = -1; it <= 2; it++) {
68  ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
69  ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
70  }
71  }
72  CompoInitialised = true;
73 }
75 {
76  initCompo();
77 
78  // std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
79 
80  dW = 0;
81 
82  if (blur)
86 
87  // double erreur=0;
88  int Nbpoint = 0;
89 
90  // double Tij;
91  double IW, dx, dy;
92  int cr, ct;
93  double er, et;
94 
95  Nbpoint = 0;
96  // erreur=0;
97 
99 
100  Warp->computeCoeff(p);
101  for (unsigned int point = 0; point < templateSize; point++) {
102  int i = ptTemplate[point].y;
103  int j = ptTemplate[point].x;
104  X1[0] = j;
105  X1[1] = i;
106 
107  Warp->computeDenom(X1, p);
108  Warp->warpX(X1, X2, p);
109 
110  double j2 = X2[0];
111  double i2 = X2[1];
112 
113  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
114  Nbpoint++;
115  // Tij=ptTemplate[point].val;
116  if (!blur)
117  IW = I.getValue(i2, j2);
118  else
119  IW = BI.getValue(i2, j2);
120 
121  dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
122  dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
123 
124  cr = ptTemplateSupp[point].ct;
125  er = ptTemplateSupp[point].et;
126  ct = (int)((IW * (Nc - 1)) / 255.);
127  et = ((double)IW * (Nc - 1)) / 255. - ct;
128 
129  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
130 
131  double *tptemp = new double[nbParam];
132  for (unsigned int it = 0; it < nbParam; it++)
133  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
134 
135  // calcul de l'erreur
136  // erreur+=(Tij-IW)*(Tij-IW);
137 
138  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
139 
140  delete[] tptemp;
141  }
142  }
143  double MI;
144  computeProba(Nbpoint);
145  computeMI(MI);
147 
148  lambda = lambdaDep;
149 
152  // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
153  // std::cout<<"\tEnd initialisation..."<<std::endl;
154 }
155 
157 {
158  if (!CompoInitialised)
159  std::cout << "Compositionnal tracking no initialised\nUse "
160  "initCompo(vpImage<unsigned char> &I) function"
161  << std::endl;
162  dW = 0;
163 
164  if (blur)
168 
169  // double erreur=0;
170 
171  lambda = lambdaDep;
172  double MI = 0, MIprec = -1000;
173 
174  MI_preEstimation = -getCost(I, p);
175 
176  double i2, j2;
177  // double Tij;
178  double IW;
179  // unsigned
180  int cr, ct;
181  double er, et;
182  double dx, dy;
183 
184  vpColVector dpinv(nbParam);
185  double alpha = 2.;
186 
187  int i, j;
188  unsigned int iteration = 0;
189  do {
190  int Nbpoint = 0;
191  MIprec = MI;
192  MI = 0;
193  // erreur=0;
194 
196 
197  Warp->computeCoeff(p);
198 
199  for (unsigned int point = 0; point < templateSize; point++) {
200  i = ptTemplate[point].y;
201  j = ptTemplate[point].x;
202  X1[0] = j;
203  X1[1] = i;
204  Warp->warpX(i, j, i2, j2, p);
205  X2[0] = j2;
206  X2[1] = i2;
207 
208  Warp->computeDenom(X1, p);
209  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
210  Nbpoint++;
211  // Tij=ptTemplate[point].val;
212  if (!blur)
213  IW = I.getValue(i2, j2);
214  else
215  IW = BI.getValue(i2, j2);
216 
217  dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
218  dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
219 
220  ct = (int)((IW * (Nc - 1)) / 255.);
221  et = ((double)IW * (Nc - 1)) / 255. - ct;
222  cr = ptTemplateSupp[point].ct;
223  er = ptTemplateSupp[point].et;
224 
225  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
226 
227  double *tptemp = new double[nbParam];
228  for (unsigned int it = 0; it < nbParam; it++)
229  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
230 
231  // calcul de l'erreur
232  // erreur+=(Tij-IW)*(Tij-IW);
233 
235  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
237  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
238 
239  delete[] tptemp;
240  }
241  }
242  if (Nbpoint == 0) {
243  // std::cout<<"plus de point dans template suivi"<<std::endl;
244  diverge = true;
245  MI = 0;
246  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
247  } else {
248  computeProba(Nbpoint);
249  computeMI(MI);
251  computeHessien(H);
252  computeGradient();
253 
255 
256  try {
257  switch (hessianComputation) {
259  dp = gain * HLMdesireInverse * G;
260  break;
262  if (HLM.cond() > HLMdesire.cond())
263  dp = gain * HLMdesireInverse * G;
264  else
265  dp = gain * 0.2 * HLM.inverseByLU() * G;
266  break;
267  default:
268  dp = gain * 0.2 * HLM.inverseByLU() * G;
269  break;
270  }
271  } catch (const vpException &e) {
272  // std::cerr<<"probleme inversion"<<std::endl;
273  throw(e);
274  }
275  }
276 
278  dp = -0.04 * dp;
279  else
280  dp = 1. * dp;
281 
282  if (useBrent) {
283  alpha = 2.;
284  computeOptimalBrentGain(I, p, -MI, dp, alpha);
285  dp = alpha * dp;
286  }
287  Warp->pRondp(p, dp, p);
288 
289  iteration++;
290 
291  } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
292  (iteration < iterationMax));
293  // while( (MI!=MIprec) && (iteration< iterationMax) );
294  nbIteration = iteration;
295 
296  MI_postEstimation = -getCost(I, p);
298  MI_postEstimation = -1;
299  }
300 }
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)=0
void computeHessien(vpMatrix &H)
void initHessienDesired(const vpImage< unsigned char > &I)
unsigned int getWidth() const
Definition: vpImage.h:239
vpTemplateTrackerPoint * ptTemplate
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
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
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
vpImage< double > BI
vpHessienApproximationType ApproxHessian
double cond() const
Definition: vpMatrix.cpp:5045
unsigned int templateSize
unsigned int iterationMax
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1420
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)
vpImage< double > dIy
unsigned int nbIteration
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
unsigned int getHeight() const
Definition: vpImage.h:178
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
virtual void getdWdp0(const int &i, const int &j, double *dIdW)=0
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5071
void computeProba(int &nbpoint)
vpHessienType hessianComputation