42 #include <visp3/core/vpImageFilter.h>
43 #include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
56 for (
unsigned int point = 0; point <
templateSize; point++) {
86 Warp->computeCoeff(
p);
87 double Ic, dIcx = 0., dIcy = 0.;
101 for (
unsigned int point = 0; point <
templateSize; point++) {
112 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
124 for (
unsigned int it = 0; it <
nbParam; it++)
128 double *tempt =
new double[
nbParam];
129 for (
unsigned int it = 0; it <
nbParam; it++)
130 tempt[it] =
dW[0][it] * dIcx +
dW[1][it] * dIcy;
131 double d_Ixx = dIxx.
getValue(i2, j2);
132 double d_Iyy = dIyy.
getValue(i2, j2);
133 double d_Ixy = dIxy.
getValue(i2, j2);
135 for (
unsigned int it = 0; it <
nbParam; it++)
136 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
137 moyd2Iref[it][jt] += (
dW[0][it] * (
dW[0][jt] * d_Ixx +
dW[1][jt] * d_Ixy) +
138 dW[1][it] * (
dW[0][jt] * d_Ixy +
dW[1][jt] * d_Iyy));
145 moyIref = moyIref / Nbpoint;
147 moyd2Iref = moyd2Iref / Nbpoint;
148 moyIc = moyIc / Nbpoint;
150 double covarIref = 0, covarIc = 0;
158 for (
unsigned int point = 0; point <
templateSize; point++) {
171 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
184 double *tempt =
new double[
nbParam];
185 for (
unsigned int it = 0; it <
nbParam; it++) {
186 tempt[it] =
dW[0][it] * dIcx +
dW[1][it] * dIcy;
189 double prodIc = (Ic - moyIc);
191 double d_Ixx = dIxx.
getValue(i2, j2);
192 double d_Iyy = dIyy.
getValue(i2, j2);
193 double d_Ixy = dIxy.
getValue(i2, j2);
195 for (
unsigned int it = 0; it <
nbParam; it++) {
196 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
197 sIcd2Iref[it][jt] += prodIc * (
dW[0][it] * (
dW[0][jt] * d_Ixx +
dW[1][jt] * d_Ixy) +
198 dW[1][it] * (
dW[0][jt] * d_Ixy +
dW[1][jt] * d_Iyy) - moyd2Iref[it][jt]);
199 sdIrefdIref[it][jt] +=
206 for (
unsigned int it = 0; it <
nbParam; it++)
209 covarIref += (Iref - moyIref) * (Iref - moyIref);
210 covarIc += (Ic - moyIc) * (Ic - moyIc);
211 sIcIref += (Iref - moyIref) * (Ic - moyIc);
214 covarIref = sqrt(covarIref);
215 covarIc = sqrt(covarIc);
217 denom = covarIref * covarIc;
219 double NCC = sIcIref / denom;
221 dcovarIref = -sIcdIref / covarIref;
224 dNCC = (sIcdIref / denom - NCC * dcovarIref / covarIref);
226 d2covarIref = -(sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.
t()) / covarIref;
230 Hdesire = (sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.
t()) / denom;
244 unsigned int iteration = 0;
249 double evolRMS_init = 0;
250 double evolRMS_prec = 0;
251 double evolRMS_delta;
254 unsigned int Nbpoint = 0;
256 Warp->computeCoeff(
p);
259 for (
unsigned int point = 0; point <
templateSize; point++) {
270 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
284 moyIref = moyIref / Nbpoint;
285 moyIc = moyIc / Nbpoint;
287 double covarIref = 0, covarIc = 0;
293 for (
unsigned int point = 0; point <
templateSize; point++) {
304 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
312 double prod = (Ic - moyIc);
313 for (
unsigned int it = 0; it <
nbParam; it++)
315 for (
unsigned int it = 0; it <
nbParam; it++)
318 covarIref += (Iref - moyIref) * (Iref - moyIref);
319 covarIc += (Ic - moyIc) * (Ic - moyIc);
320 sIcIref += (Iref - moyIref) * (Ic - moyIc);
323 covarIref = sqrt(covarIref);
324 covarIc = sqrt(covarIc);
325 double denom = covarIref * covarIc;
327 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
330 double NCC = sIcIref / denom;
332 dcovarIref = sIrefdIref / covarIref;
333 G = (sIcdIref / denom - NCC * dcovarIref / covarIref);
350 if (iteration == 0) {
355 evolRMS_delta = std::fabs(
evolRMS - evolRMS_prec);