Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
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
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 {
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
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 getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradXGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIx, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void getGradX(const vpImage< unsigned char > &I, vpImage< FilterType > &dIx)
static void filter(const vpImage< unsigned char > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
static void getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type getValue(unsigned int i, unsigned int j) const
Definition vpImage.h:1592
unsigned int getHeight() const
Definition vpImage.h:184
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
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)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
vpImage< double > BI
unsigned int templateSize