Visual Servoing Platform  version 3.1.0
vpTemplateTrackerZNCCForwardAdditional.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Template tracker.
33  *
34  * Authors:
35  * Amaury Dame
36  * Aurelien Yol
37  * Fabien Spindler
38  *
39  *****************************************************************************/
40 #include <visp3/core/vpImageFilter.h>
41 #include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
42 
44  : vpTemplateTrackerZNCC(warp)
45 {
46  useCompositionnal = false;
47 }
48 
50 {
51  if (blur)
55 
56  vpImage<double> dIxx, dIxy, dIyx, dIyy;
59 
62 
63  Warp->computeCoeff(p);
64  double IW, dIWx, dIWy;
65  double Tij;
66  int i, j;
67  double i2, j2;
68  int Nbpoint = 0;
69 
70  double moyTij = 0;
71  double moyIW = 0;
72  double denom = 0;
73  for (unsigned int point = 0; point < templateSize; point++) {
74  i = ptTemplate[point].y;
75  j = ptTemplate[point].x;
76  X1[0] = j;
77  X1[1] = i;
78  X2[0] = j;
79  X2[1] = i;
80 
81  Warp->computeDenom(X1, p);
82 
83  j2 = X2[0];
84  i2 = X2[1];
85 
86  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
87  Tij = ptTemplate[point].val;
88 
89  if (!blur)
90  IW = I.getValue(i2, j2);
91  else
92  IW = BI.getValue(i2, j2);
93 
94  Nbpoint++;
95  moyTij += Tij;
96  moyIW += IW;
97  }
98  }
99  moyTij = moyTij / Nbpoint;
100  moyIW = moyIW / Nbpoint;
101  Hdesire = 0;
102  for (unsigned int point = 0; point < templateSize; point++) {
103  i = ptTemplate[point].y;
104  j = ptTemplate[point].x;
105  X1[0] = j;
106  X1[1] = i;
107  X2[0] = j;
108  X2[1] = i;
109 
110  Warp->computeDenom(X1, p);
111 
112  j2 = X2[0];
113  i2 = X2[1];
114 
115  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
116  Tij = ptTemplate[point].val;
117 
118  if (!blur)
119  IW = I.getValue(i2, j2);
120  else
121  IW = BI.getValue(i2, j2);
122 
123  dIWx = dIx.getValue(i2, j2);
124  dIWy = dIy.getValue(i2, j2);
125  // Calcul du Hessien
126  Warp->dWarp(X1, X2, p, dW);
127  double *tempt = new double[nbParam];
128  for (unsigned int it = 0; it < nbParam; it++)
129  tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
130 
131  double prod = (Tij - moyTij);
132 
133  double d_Ixx = dIxx.getValue(i2, j2);
134  double d_Iyy = dIyy.getValue(i2, j2);
135  double d_Ixy = dIxy.getValue(i2, j2);
136 
137  for (unsigned int it = 0; it < nbParam; it++)
138  for (unsigned int jt = 0; jt < nbParam; jt++)
139  Hdesire[it][jt] += prod * (dW[0][it] * (dW[0][jt] * d_Ixx + dW[1][jt] * d_Ixy) +
140  dW[1][it] * (dW[0][jt] * d_Ixy + dW[1][jt] * d_Iyy));
141  /*Hdesire[0][0]+=prod*d_Ixx;
142  Hdesire[1][0]+=prod*d_Ixy;
143  Hdesire[0][1]+=prod*d_Ixy;
144  Hdesire[1][1]+=prod*d_Iyy;*/
145 
146  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
147  delete[] tempt;
148  }
149  }
150 
151  Hdesire = Hdesire / sqrt(denom);
154  // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
155 }
156 
158 {
159  if (blur)
163 
164  /*vpImage<double> dIxx,dIxy,dIyx,dIyy;
165  getGradX(dIx, dIxx, fgdG,taillef);
166  getGradY(dIx, dIxy, fgdG,taillef);
167 
168  getGradX(dIy, dIyx, fgdG,taillef);
169  getGradY(dIy, dIyy, fgdG,taillef);*/
170 
171  dW = 0;
172 
173  // double lambda=lambdaDep;
174  double IW, dIWx, dIWy;
175  double Tij;
176  unsigned int iteration = 0;
177  int i, j;
178  double i2, j2;
179  double alpha = 2.;
180  do {
181  int Nbpoint = 0;
182  double erreur = 0;
183  G = 0;
184  H = 0;
185  Warp->computeCoeff(p);
186  double moyTij = 0;
187  double moyIW = 0;
188  double denom = 0;
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 
195  Warp->computeDenom(X1, p);
196  Warp->warpX(X1, X2, p);
197 
198  j2 = X2[0];
199  i2 = X2[1];
200  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
201  Tij = ptTemplate[point].val;
202 
203  if (!blur)
204  IW = I.getValue(i2, j2);
205  else
206  IW = BI.getValue(i2, j2);
207 
208  Nbpoint++;
209  moyTij += Tij;
210  moyIW += IW;
211  }
212  }
213 
214  if (!Nbpoint) {
215  throw(vpException(vpException::divideByZeroError, "Cannot track the template: no point"));
216  }
217 
218  moyTij = moyTij / Nbpoint;
219  moyIW = moyIW / Nbpoint;
220  // vpMatrix d2Wx(nbParam,nbParam);
221  // vpMatrix d2Wy(nbParam,nbParam);
222  for (unsigned int point = 0; point < templateSize; point++) {
223  i = ptTemplate[point].y;
224  j = ptTemplate[point].x;
225  X1[0] = j;
226  X1[1] = i;
227 
228  Warp->computeDenom(X1, p);
229  Warp->warpX(X1, X2, p);
230 
231  j2 = X2[0];
232  i2 = X2[1];
233  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
234  Tij = ptTemplate[point].val;
235 
236  if (!blur)
237  IW = I.getValue(i2, j2);
238  else
239  IW = BI.getValue(i2, j2);
240 
241  dIWx = dIx.getValue(i2, j2);
242  dIWy = dIy.getValue(i2, j2);
243  // Calcul du Hessien
244  Warp->dWarp(X1, X2, p, dW);
245  double *tempt = new double[nbParam];
246  for (unsigned int it = 0; it < nbParam; it++)
247  tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
248 
249  double prod = (Tij - moyTij);
250  for (unsigned int it = 0; it < nbParam; it++)
251  G[it] += prod * tempt[it];
252 
253  /* Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
254  for(int it=0;it<nbParam;it++)
255  for(int jt=0;jt<nbParam;jt++)
256  H[it][jt]+=prod*(d2Wx[it][jt]*dIWx+d2Wx[it][jt]*dIWy);*/
257  /*double d_Ixx=dIxx.getValue(i2,j2);
258  double d_Iyy=dIyy.getValue(i2,j2);
259  double d_Ixy=dIxy.getValue(i2,j2);
260 
261  for(int it=0;it<nbParam;it++)
262  for(int jt=0;jt<nbParam;jt++)
263  H[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
264  +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));*/
265  /*H[0][0]+=prod*d_Ixx;
266  H[1][0]+=prod*d_Ixy;
267  H[0][1]+=prod*d_Ixy;
268  H[1][1]+=prod*d_Iyy;*/
269 
270  double er = (Tij - IW);
271  erreur += (er * er);
272  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
273  delete[] tempt;
274  }
275  }
276  /*std::cout<<"G="<<G<<std::endl;
277  std::cout<<"H="<<H<<std::endl;
278  std::cout<<" denom="<<denom<<std::endl;*/
279  G = G / sqrt(denom);
280  // std::cout<<G<<std::endl;
281  H = H / sqrt(denom);
282 
283  // if(Nbpoint==0)std::cout<<"plus de point dans template
284  // suivi"<<std::endl; // cannot occur
285 
286  try {
287  // vpMatrix::computeHLM(H,lambda,HLM);
288  // dp=1.*HLM.inverseByLU()*G;
289  dp = 1. * HLMdesireInverse * G;
290  } catch (vpException &e) {
291  // std::cout<<"probleme inversion"<<std::endl;
292  throw(e);
293  }
294 
295  dp = gain * dp;
296  if (useBrent) {
297  alpha = 2.;
298  computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
299  dp = alpha * dp;
300  }
301  p -= dp;
302  iteration++;
303  } while (/*( erreur_prec-erreur<50) && */ (iteration < iterationMax));
304 
305  // std::cout<<"erreur "<<erreur<<std::endl;
306  nbIteration = iteration;
307 }
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)=0
vpMatrix inverseByLU() const
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
unsigned int templateSize
unsigned int iterationMax
vpImage< double > dIx
vpImage< double > dIy
Type getValue(double i, double j) const
Definition: vpImage.h:1338
unsigned int nbIteration
unsigned int getHeight() const
Definition: vpImage.h:178
void initHessienDesired(const vpImage< unsigned char > &I)
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, const bool convolve=false)
vpTemplateTrackerWarp * Warp
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:5013
unsigned int getWidth() const
Definition: vpImage.h:229
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW)=0