Visual Servoing Platform  version 3.3.1 under development (2020-10-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  ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
50  for (unsigned int point = 0; point < templateSize; point++) {
51  int i = ptTemplate[point].y;
52  int j = ptTemplate[point].x;
53  X1[0] = j;
54  X1[1] = i;
55  Warp->computeDenom(X1, p);
56  ptTemplate[point].dW = new double[2 * nbParam];
57  Warp->getdWdp0(i, j, ptTemplate[point].dW);
58 
59  double Tij = ptTemplate[point].val;
60  int ct = (int)((Tij * (Nc - 1)) / 255.);
61  double et = (Tij * (Nc - 1)) / 255. - ct;
62  ptTemplateSupp[point].et = et;
63  ptTemplateSupp[point].ct = ct;
64  ptTemplateSupp[point].Bt = new double[4];
65  ptTemplateSupp[point].dBt = new double[4];
66  for (char it = -1; it <= 2; it++) {
67  ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
68  ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
69  }
70  }
71  CompoInitialised = true;
72 }
74 {
75  initCompo();
76 
77  dW = 0;
78 
79  if (blur)
83 
84  int Nbpoint = 0;
85 
86  double IW, dx, dy;
87  int cr, ct;
88  double er, et;
89 
90  Nbpoint = 0;
91 
93 
94  Warp->computeCoeff(p);
95  for (unsigned int point = 0; point < templateSize; point++) {
96  int i = ptTemplate[point].y;
97  int j = ptTemplate[point].x;
98  X1[0] = j;
99  X1[1] = i;
100 
101  Warp->computeDenom(X1, p);
102  Warp->warpX(X1, X2, p);
103 
104  double j2 = X2[0];
105  double i2 = X2[1];
106 
107  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
108  Nbpoint++;
109  if (!blur)
110  IW = I.getValue(i2, j2);
111  else
112  IW = BI.getValue(i2, j2);
113 
114  dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
115  dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
116 
117  cr = ptTemplateSupp[point].ct;
118  er = ptTemplateSupp[point].et;
119  ct = (int)((IW * (Nc - 1)) / 255.);
120  et = ((double)IW * (Nc - 1)) / 255. - ct;
121 
122  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
123 
124  double *tptemp = new double[nbParam];
125  for (unsigned int it = 0; it < nbParam; it++)
126  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
127 
128  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
129 
130  delete[] tptemp;
131  }
132  }
133  double MI;
134  computeProba(Nbpoint);
135  computeMI(MI);
137 
138  lambda = lambdaDep;
139 
142 }
143 
145 {
146  if (!CompoInitialised) {
147  std::cout << "Compositionnal tracking not initialised.\nUse initCompo() function." << std::endl;
148  }
149  dW = 0;
150 
151  if (blur) {
153  }
156 
157  lambda = lambdaDep;
158  double MI = 0, MIprec = -1000;
159 
160  MI_preEstimation = -getCost(I, p);
161 
162  initPosEvalRMS(p);
163 
164  double i2, j2;
165  double IW;
166  int cr, ct;
167  double er, et;
168  double dx, dy;
169 
170  vpColVector dpinv(nbParam);
171  double alpha = 2.;
172 
173  int i, j;
174  unsigned int iteration = 0;
175 
176  double evolRMS_init = 0;
177  double evolRMS_prec = 0;
178  double evolRMS_delta;
179 
180  do {
181  int Nbpoint = 0;
182  MIprec = MI;
183  MI = 0;
184 
186 
187  Warp->computeCoeff(p);
188 
189  for (unsigned int point = 0; point < templateSize; point++) {
190  i = ptTemplate[point].y;
191  j = ptTemplate[point].x;
192  X1[0] = j;
193  X1[1] = i;
194  Warp->warpX(i, j, i2, j2, p);
195  X2[0] = j2;
196  X2[1] = i2;
197 
198  Warp->computeDenom(X1, p);
199  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
200  Nbpoint++;
201  if (!blur)
202  IW = I.getValue(i2, j2);
203  else
204  IW = BI.getValue(i2, j2);
205 
206  dx = dIx.getValue(i2, j2) * (Nc - 1) / 255.;
207  dy = dIy.getValue(i2, j2) * (Nc - 1) / 255.;
208 
209  ct = (int)((IW * (Nc - 1)) / 255.);
210  et = ((double)IW * (Nc - 1)) / 255. - ct;
211  cr = ptTemplateSupp[point].ct;
212  er = ptTemplateSupp[point].et;
213 
214  Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
215 
216  double *tptemp = new double[nbParam];
217  for (unsigned int it = 0; it < nbParam; it++)
218  tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
219 
221  vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
223  vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
224 
225  delete[] tptemp;
226  }
227  }
228  if (Nbpoint == 0) {
229  diverge = true;
230  MI = 0;
231  throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
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  } catch (const vpException &e) {
257  throw(e);
258  }
259  }
260 
262  dp = -0.04 * dp;
263 // else
264 // dp = 1. * dp;
265 
266  if (useBrent) {
267  alpha = 2.;
268  computeOptimalBrentGain(I, p, -MI, dp, alpha);
269  dp = alpha * dp;
270  }
271  Warp->pRondp(p, dp, p);
272  computeEvalRMS(p);
273 
274  if (iteration == 0) {
275  evolRMS_init = evolRMS;
276  }
277 
278  iteration++;
279 
280  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
281  evolRMS_prec = evolRMS;
282 
283  } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
284  (iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init)*evolRMS_eps) );
285 
286  nbIteration = iteration;
287 
288  MI_postEstimation = -getCost(I, p);
290  MI_postEstimation = -1;
291  }
292 }
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
void computeHessien(vpMatrix &H)
void initHessienDesired(const vpImage< unsigned char > &I)
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
vpTemplateTrackerPoint * ptTemplate
vpMatrix inverseByLU() const
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, const double *dwdp0, vpMatrix &dM)=0
error that can be emited by ViSP classes.
Definition: vpException.h:71
void initPosEvalRMS(const vpColVector &p)
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 > BI
vpHessienApproximationType ApproxHessian
unsigned int templateSize
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:6718
virtual void getdWdp0(const int &v, const int &u, double *dIdW)=0
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
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
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6779
unsigned int getWidth() const
Definition: vpImage.h:246
void computeProba(int &nbpoint)
vpHessienType hessianComputation