Visual Servoing Platform  version 3.3.1 under development (2020-10-22)
vpTemplateTrackerZNCCForwardAdditional.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  * 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  double *tempt = new double[nbParam];
74 
75  for (unsigned int point = 0; point < templateSize; point++) {
76  i = ptTemplate[point].y;
77  j = ptTemplate[point].x;
78  X1[0] = j;
79  X1[1] = i;
80  X2[0] = j;
81  X2[1] = i;
82 
83  Warp->computeDenom(X1, p);
84 
85  j2 = X2[0];
86  i2 = X2[1];
87 
88  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
89  Tij = ptTemplate[point].val;
90 
91  if (!blur)
92  IW = I.getValue(i2, j2);
93  else
94  IW = BI.getValue(i2, j2);
95 
96  Nbpoint++;
97  moyTij += Tij;
98  moyIW += IW;
99  }
100  }
101  moyTij = moyTij / Nbpoint;
102  moyIW = moyIW / Nbpoint;
103  Hdesire = 0;
104  for (unsigned int point = 0; point < templateSize; point++) {
105  i = ptTemplate[point].y;
106  j = ptTemplate[point].x;
107  X1[0] = j;
108  X1[1] = i;
109  X2[0] = j;
110  X2[1] = i;
111 
112  Warp->computeDenom(X1, p);
113 
114  j2 = X2[0];
115  i2 = X2[1];
116 
117  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
118  Tij = ptTemplate[point].val;
119 
120  if (!blur)
121  IW = I.getValue(i2, j2);
122  else
123  IW = BI.getValue(i2, j2);
124 
125  dIWx = dIx.getValue(i2, j2);
126  dIWy = dIy.getValue(i2, j2);
127  // Calcul du Hessien
128  Warp->dWarp(X1, X2, p, dW);
129  for (unsigned int it = 0; it < nbParam; it++)
130  tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
131 
132  double prod = (Tij - moyTij);
133 
134  double d_Ixx = dIxx.getValue(i2, j2);
135  double d_Iyy = dIyy.getValue(i2, j2);
136  double d_Ixy = dIxy.getValue(i2, j2);
137 
138  for (unsigned int it = 0; it < nbParam; it++)
139  for (unsigned int jt = 0; jt < nbParam; jt++)
140  Hdesire[it][jt] += prod * (dW[0][it] * (dW[0][jt] * d_Ixx + dW[1][jt] * d_Ixy) +
141  dW[1][it] * (dW[0][jt] * d_Ixy + dW[1][jt] * d_Iyy));
142  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
143  }
144  }
145  delete[] tempt;
146 
147  Hdesire = Hdesire / sqrt(denom);
150 }
151 
153 {
154  if (blur)
158 
159  dW = 0;
160 
161  double IW, dIWx, dIWy;
162  double Tij;
163  unsigned int iteration = 0;
164  int i, j;
165  double i2, j2;
166  double alpha = 2.;
167 
168  initPosEvalRMS(p);
169 
170  double evolRMS_init = 0;
171  double evolRMS_prec = 0;
172  double evolRMS_delta;
173  double *tempt = new double[nbParam];
174 
175  do {
176  int Nbpoint = 0;
177  double erreur = 0;
178  G = 0;
179  H = 0;
180  Warp->computeCoeff(p);
181  double moyTij = 0;
182  double moyIW = 0;
183  double denom = 0;
184  for (unsigned int point = 0; point < templateSize; point++) {
185  i = ptTemplate[point].y;
186  j = ptTemplate[point].x;
187  X1[0] = j;
188  X1[1] = i;
189 
190  Warp->computeDenom(X1, p);
191  Warp->warpX(X1, X2, p);
192 
193  j2 = X2[0];
194  i2 = X2[1];
195  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
196  Tij = ptTemplate[point].val;
197 
198  if (!blur)
199  IW = I.getValue(i2, j2);
200  else
201  IW = BI.getValue(i2, j2);
202 
203  Nbpoint++;
204  moyTij += Tij;
205  moyIW += IW;
206  }
207  }
208 
209  if (!Nbpoint) {
210  delete[] tempt;
211  throw(vpException(vpException::divideByZeroError, "Cannot track the template: no point"));
212  }
213 
214  moyTij = moyTij / Nbpoint;
215  moyIW = moyIW / Nbpoint;
216  for (unsigned int point = 0; point < templateSize; point++) {
217  i = ptTemplate[point].y;
218  j = ptTemplate[point].x;
219  X1[0] = j;
220  X1[1] = i;
221 
222  Warp->computeDenom(X1, p);
223  Warp->warpX(X1, X2, p);
224 
225  j2 = X2[0];
226  i2 = X2[1];
227  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
228  Tij = ptTemplate[point].val;
229 
230  if (!blur)
231  IW = I.getValue(i2, j2);
232  else
233  IW = BI.getValue(i2, j2);
234 
235  dIWx = dIx.getValue(i2, j2);
236  dIWy = dIy.getValue(i2, j2);
237  // Calcul du Hessien
238  Warp->dWarp(X1, X2, p, dW);
239  for (unsigned int it = 0; it < nbParam; it++)
240  tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
241 
242  double prod = (Tij - moyTij);
243  for (unsigned int it = 0; it < nbParam; it++)
244  G[it] += prod * tempt[it];
245 
246  double er = (Tij - IW);
247  erreur += (er * er);
248  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
249  }
250  }
251  G = G / sqrt(denom);
252  H = H / sqrt(denom);
253 
254  try {
255  dp = HLMdesireInverse * G;
256  } catch (const vpException &e) {
257  delete[] tempt;
258  throw(e);
259  }
260 
261  dp = gain * dp;
262  if (useBrent) {
263  alpha = 2.;
264  computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
265  dp = alpha * dp;
266  }
267  p -= dp;
268 
269  computeEvalRMS(p);
270 
271  if (iteration == 0) {
272  evolRMS_init = evolRMS;
273  }
274  iteration++;
275 
276  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
277  evolRMS_prec = evolRMS;
278 
279  } while ((iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init)*evolRMS_eps));
280  delete[] tempt;
281 
282  nbIteration = iteration;
283 }
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
vpTemplateTrackerPoint * ptTemplate
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
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
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
unsigned int templateSize
unsigned int iterationMax
vpImage< double > dIx
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
void initHessienDesired(const vpImage< unsigned char > &I)
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
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