Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpTemplateTrackerZNCCForwardAdditional.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  *
38 *****************************************************************************/
39 #include <visp3/core/vpImageFilter.h>
40 #include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
41 
43  : vpTemplateTrackerZNCC(warp)
44 {
45  useCompositionnal = false;
46 }
47 
49 {
50  if (blur)
54 
55  vpImage<double> dIxx, dIxy, dIyx, dIyy;
58 
61 
62  Warp->computeCoeff(p);
63  double IW, dIWx, dIWy;
64  double Tij;
65  int i, j;
66  double i2, j2;
67  int Nbpoint = 0;
68 
69  double moyTij = 0;
70  double moyIW = 0;
71  double denom = 0;
72  double *tempt = new double[nbParam];
73 
74  for (unsigned int point = 0; point < templateSize; point++) {
75  i = ptTemplate[point].y;
76  j = ptTemplate[point].x;
77  X1[0] = j;
78  X1[1] = i;
79  X2[0] = j;
80  X2[1] = i;
81 
82  Warp->computeDenom(X1, p);
83 
84  j2 = X2[0];
85  i2 = X2[1];
86 
87  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
88  Tij = ptTemplate[point].val;
89 
90  if (!blur)
91  IW = I.getValue(i2, j2);
92  else
93  IW = BI.getValue(i2, j2);
94 
95  Nbpoint++;
96  moyTij += Tij;
97  moyIW += IW;
98  }
99  }
100  moyTij = moyTij / Nbpoint;
101  moyIW = moyIW / Nbpoint;
102  Hdesire = 0;
103  for (unsigned int point = 0; point < templateSize; point++) {
104  i = ptTemplate[point].y;
105  j = ptTemplate[point].x;
106  X1[0] = j;
107  X1[1] = i;
108  X2[0] = j;
109  X2[1] = i;
110 
111  Warp->computeDenom(X1, p);
112 
113  j2 = X2[0];
114  i2 = X2[1];
115 
116  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
117  Tij = ptTemplate[point].val;
118 
119  if (!blur)
120  IW = I.getValue(i2, j2);
121  else
122  IW = BI.getValue(i2, j2);
123 
124  dIWx = dIx.getValue(i2, j2);
125  dIWy = dIy.getValue(i2, j2);
126  // Calcul du Hessien
127  Warp->dWarp(X1, X2, p, dW);
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  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
142  }
143  }
144  delete[] tempt;
145 
146  Hdesire = Hdesire / sqrt(denom);
149 }
150 
152 {
153  if (blur)
157 
158  dW = 0;
159 
160  double IW, dIWx, dIWy;
161  double Tij;
162  unsigned int iteration = 0;
163  int i, j;
164  double i2, j2;
165  double alpha = 2.;
166 
167  initPosEvalRMS(p);
168 
169  double evolRMS_init = 0;
170  double evolRMS_prec = 0;
171  double evolRMS_delta;
172  double *tempt = new double[nbParam];
173 
174  do {
175  int Nbpoint = 0;
176  double erreur = 0;
177  G = 0;
178  H = 0;
179  Warp->computeCoeff(p);
180  double moyTij = 0;
181  double moyIW = 0;
182  double denom = 0;
183  for (unsigned int point = 0; point < templateSize; point++) {
184  i = ptTemplate[point].y;
185  j = ptTemplate[point].x;
186  X1[0] = j;
187  X1[1] = i;
188 
189  Warp->computeDenom(X1, p);
190  Warp->warpX(X1, X2, p);
191 
192  j2 = X2[0];
193  i2 = X2[1];
194  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
195  Tij = ptTemplate[point].val;
196 
197  if (!blur)
198  IW = I.getValue(i2, j2);
199  else
200  IW = BI.getValue(i2, j2);
201 
202  Nbpoint++;
203  moyTij += Tij;
204  moyIW += IW;
205  }
206  }
207 
208  if (!Nbpoint) {
209  delete[] tempt;
210  throw(vpException(vpException::divideByZeroError, "Cannot track the template: no point"));
211  }
212 
213  moyTij = moyTij / Nbpoint;
214  moyIW = moyIW / Nbpoint;
215  for (unsigned int point = 0; point < templateSize; point++) {
216  i = ptTemplate[point].y;
217  j = ptTemplate[point].x;
218  X1[0] = j;
219  X1[1] = i;
220 
221  Warp->computeDenom(X1, p);
222  Warp->warpX(X1, X2, p);
223 
224  j2 = X2[0];
225  i2 = X2[1];
226  if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
227  Tij = ptTemplate[point].val;
228 
229  if (!blur)
230  IW = I.getValue(i2, j2);
231  else
232  IW = BI.getValue(i2, j2);
233 
234  dIWx = dIx.getValue(i2, j2);
235  dIWy = dIy.getValue(i2, j2);
236  // Calcul du Hessien
237  Warp->dWarp(X1, X2, p, dW);
238  for (unsigned int it = 0; it < nbParam; it++)
239  tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
240 
241  double prod = (Tij - moyTij);
242  for (unsigned int it = 0; it < nbParam; it++)
243  G[it] += prod * tempt[it];
244 
245  double er = (Tij - IW);
246  erreur += (er * er);
247  denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
248  }
249  }
250  G = G / sqrt(denom);
251  H = H / sqrt(denom);
252 
253  try {
254  dp = HLMdesireInverse * G;
255  } catch (const vpException &e) {
256  delete[] tempt;
257  throw(e);
258  }
259 
260  dp = gain * dp;
261  if (useBrent) {
262  alpha = 2.;
263  computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
264  dp = alpha * dp;
265  }
266  p -= dp;
267 
268  computeEvalRMS(p);
269 
270  if (iteration == 0) {
271  evolRMS_init = evolRMS;
272  }
273  iteration++;
274 
275  evolRMS_delta = std::fabs(evolRMS - evolRMS_prec);
276  evolRMS_prec = evolRMS;
277 
278  } while ((iteration < iterationMax) && (evolRMS_delta > std::fabs(evolRMS_init) * evolRMS_eps));
279  delete[] tempt;
280 
281  nbIteration = iteration;
282 }
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
static void getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx, const vpImage< bool > *p_mask=nullptr)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false, const vpImage< bool > *p_mask=nullptr)
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size, const vpImage< bool > *p_mask=nullptr)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy, const vpImage< bool > *p_mask=nullptr)
unsigned int getWidth() const
Definition: vpImage.h:249
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1816
unsigned int getHeight() const
Definition: vpImage.h:184
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6407
virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &p, vpMatrix &dM)=0
virtual void warpX(const int &v1, const int &u1, double &v2, double &u2, const vpColVector &p)=0
void initHessienDesired(const vpImage< unsigned char > &I)
vpImage< double > dIx
vpImage< double > dIy
void computeEvalRMS(const vpColVector &p)
void computeOptimalBrentGain(const vpImage< unsigned char > &I, vpColVector &tp, double tMI, vpColVector &direction, double &alpha)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
unsigned int nbIteration
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize