Visual Servoing Platform version 3.5.0
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
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
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 {
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
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}
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
static void filter(const vpImage< double > &I, vpImage< double > &Iu, vpImage< double > &Iv, const vpMatrix &M, bool convolve=false)
static void getGradXGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradX(const vpImage< unsigned char > &I, vpImage< double > &dIx)
static void getGradYGauss2D(const vpImage< unsigned char > &I, vpImage< double > &dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
static void getGradY(const vpImage< unsigned char > &I, vpImage< double > &dIy)
unsigned int getWidth() const
Definition: vpImage.h:246
Type getValue(unsigned int i, unsigned int j) const
Definition: vpImage.h:1346
unsigned int getHeight() const
Definition: vpImage.h:188
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
Definition: vpMatrix.cpp:6683
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