Visual Servoing Platform  version 3.3.0
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 }
vpTrackingException
Error that can be emited by the vpTracker class and its derivates.
Definition: vpTrackingException.h:69
vpTemplateTracker::G
vpColVector G
Definition: vpTemplateTracker.h:105
vpTemplateTracker::taillef
unsigned int taillef
Definition: vpTemplateTracker.h:113
vpTemplateTrackerMIForwardCompositional::initHessienDesired
void initHessienDesired(const vpImage< unsigned char > &I)
Definition: vpTemplateTrackerMIForwardCompositional.cpp:73
vpTemplateTrackerWarp
Definition: vpTemplateTrackerWarp.h:58
vpTemplateTracker::initPosEvalRMS
void initPosEvalRMS(const vpColVector &p)
Definition: vpTemplateTracker.cpp:958
vpTemplateTracker::ptTemplate
vpTemplateTrackerPoint * ptTemplate
Definition: vpTemplateTracker.h:75
vpTemplateTracker::HLM
vpMatrix HLM
Definition: vpTemplateTracker.h:100
vpTemplateTrackerMI::computeGradient
void computeGradient()
Definition: vpTemplateTrackerMI.cpp:557
vpTemplateTracker::useBrent
bool useBrent
Definition: vpTemplateTracker.h:111
vpTemplateTracker::dp
vpColVector dp
Definition: vpTemplateTracker.h:133
vpTemplateTrackerWarp::dWarpCompo
virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW)=0
vpTemplateTrackerMIForwardCompositional::CompoInitialised
bool CompoInitialised
Definition: vpTemplateTrackerMIForwardCompositional.h:61
vpTemplateTracker::diverge
bool diverge
Definition: vpTemplateTracker.h:125
vpImageFilter::filter
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
Definition: vpImageFilter.cpp:127
vpTemplateTracker::X2
vpColVector X2
Definition: vpTemplateTracker.h:137
vpTemplateTrackerMI::computeProba
void computeProba(int &nbpoint)
Definition: vpTemplateTrackerMI.cpp:343
vpTemplateTracker::fgdG
double * fgdG
Definition: vpTemplateTracker.h:115
vpTemplateTrackerMIForwardCompositional::vpTemplateTrackerMIForwardCompositional
vpTemplateTrackerMIForwardCompositional(vpTemplateTrackerWarp *_warp)
Definition: vpTemplateTrackerMIForwardCompositional.cpp:42
vpTemplateTracker::computeOptimalBrentGain
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
Definition: vpTemplateTracker.cpp:392
vpTemplateTracker::fgG
double * fgG
Definition: vpTemplateTracker.h:114
vpTemplateTrackerMI
Definition: vpTemplateTrackerMI.h:53
vpTemplateTrackerMI::HESSIAN_0
@ HESSIAN_0
Definition: vpTemplateTrackerMI.h:59
vpTemplateTracker::evolRMS
double evolRMS
Definition: vpTemplateTracker.h:70
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:222
vpTemplateTracker::blur
bool blur
Definition: vpTemplateTracker.h:110
vpTemplateTracker::HLMdesire
vpMatrix HLMdesire
Definition: vpTemplateTracker.h:101
vpTemplateTrackerMI::MI_postEstimation
double MI_postEstimation
Definition: vpTemplateTrackerMI.h:100
vpTrackingException::notEnoughPointError
@ notEnoughPointError
Definition: vpTrackingException.h:80
vpTemplateTrackerMI::computeMI
void computeMI(double &MI)
Definition: vpTemplateTrackerMI.cpp:388
vpTemplateTrackerMIForwardCompositional::initCompo
void initCompo()
Definition: vpTemplateTrackerMIForwardCompositional.cpp:47
vpTemplateTrackerMI::USE_HESSIEN_BEST_COND
@ USE_HESSIEN_BEST_COND
Definition: vpTemplateTrackerMI.h:67
vpImageFilter::getGradYGauss2D
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
Definition: vpImageFilter.cpp:767
vpTemplateTrackerWarp::warpX
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
vpTemplateTracker::templateSize
unsigned int templateSize
Definition: vpTemplateTracker.h:78
vpTemplateTrackerMI::HESSIAN_NEW
@ HESSIAN_NEW
Definition: vpTemplateTrackerMI.h:63
vpMatrix::cond
double cond(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:5392
vpTemplateTracker::computeEvalRMS
void computeEvalRMS(const vpColVector &p)
Definition: vpTemplateTracker.cpp:925
vpTemplateTracker::X1
vpColVector X1
Definition: vpTemplateTracker.h:136
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpTemplateTrackerMI::MI_preEstimation
double MI_preEstimation
Definition: vpTemplateTrackerMI.h:99
vpTemplateTracker::BI
vpImage< double > BI
Definition: vpTemplateTracker.h:141
vpTemplateTracker::nbIteration
unsigned int nbIteration
Definition: vpTemplateTracker.h:126
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:280
vpTemplateTracker::dW
vpMatrix dW
Definition: vpTemplateTracker.h:139
vpTemplateTrackerMIForwardCompositional::trackNoPyr
void trackNoPyr(const vpImage< unsigned char > &I)
Definition: vpTemplateTrackerMIForwardCompositional.cpp:144
vpTemplateTracker::Hdesire
vpMatrix Hdesire
Definition: vpTemplateTracker.h:98
vpTemplateTrackerPoint::val
double val
Definition: vpTemplateTrackerHeader.h:75
vpTemplateTrackerWarp::pRondp
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const =0
vpTemplateTracker::iterationMax
unsigned int iterationMax
Definition: vpTemplateTracker.h:121
vpTemplateTracker::HLMdesireInverse
vpMatrix HLMdesireInverse
Definition: vpTemplateTracker.h:103
vpTemplateTrackerMI::PrtTout
double * PrtTout
Definition: vpTemplateTrackerMI.h:83
vpTemplateTracker::nbParam
unsigned int nbParam
Definition: vpTemplateTracker.h:119
vpTemplateTracker::dIy
vpImage< double > dIy
Definition: vpTemplateTracker.h:143
vpMatrix::inverseByLU
vpMatrix inverseByLU() const
Definition: vpMatrix_lu.cpp:134
vpTemplateTracker::H
vpMatrix H
Definition: vpTemplateTracker.h:97
vpTemplateTrackerMI::ApproxHessian
vpHessienApproximationType ApproxHessian
Definition: vpTemplateTrackerMI.h:74
vpTemplateTrackerMI::USE_HESSIEN_DESIRE
@ USE_HESSIEN_DESIRE
Definition: vpTemplateTrackerMI.h:67
vpTemplateTrackerPoint::y
int y
Definition: vpTemplateTrackerHeader.h:73
vpTemplateTracker::dIx
vpImage< double > dIx
Definition: vpTemplateTracker.h:142
vpImage::getValue
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1458
vpTemplateTrackerMI::bspline
int bspline
Definition: vpTemplateTrackerMI.h:90
vpTemplateTrackerMI::zeroProbabilities
void zeroProbabilities()
Definition: vpTemplateTrackerMI.cpp:585
vpTemplateTracker::lambdaDep
double lambdaDep
Definition: vpTemplateTracker.h:120
vpTemplateTrackerMI::getCost
double getCost(const vpImage< unsigned char > &I, const vpColVector &tp)
Definition: vpTemplateTrackerMI.cpp:148
vpTemplateTrackerPoint::dW
double * dW
Definition: vpTemplateTrackerHeader.h:76
vpTemplateTracker::p
vpColVector p
Definition: vpTemplateTracker.h:132
vpTemplateTrackerMI::HESSIAN_NONSECOND
@ HESSIAN_NONSECOND
Definition: vpTemplateTrackerMI.h:58
vpMatrix::computeHLM
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5453
vpTemplateTrackerMI::hessianComputation
vpHessienType hessianComputation
Definition: vpTemplateTrackerMI.h:73
vpTemplateTrackerMI::Nc
int Nc
Definition: vpTemplateTrackerMI.h:92
vpImage< unsigned char >
vpTemplateTracker::gain
double gain
Definition: vpTemplateTracker.h:107
vpTemplateTrackerWarp::getdWdp0
virtual void getdWdp0(const int &i, const int &j, double *dIdW)=0
vpTemplateTrackerPoint::x
int x
Definition: vpTemplateTrackerHeader.h:73
vpTemplateTrackerMI::lambda
double lambda
Definition: vpTemplateTrackerMI.h:75
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpTemplateTrackerMI::computeHessien
void computeHessien(vpMatrix &H)
Definition: vpTemplateTrackerMI.cpp:421
vpTemplateTracker::Warp
vpTemplateTrackerWarp * Warp
Definition: vpTemplateTracker.h:130
vpImageFilter::getGradXGauss2D
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
Definition: vpImageFilter.cpp:750
vpTemplateTracker::evolRMS_eps
double evolRMS_eps
Definition: vpTemplateTracker.h:73