41 #include <visp3/core/vpImageFilter.h>
42 #include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
55 for (
unsigned int point = 0; point <
templateSize; point++) {
85 Warp->computeCoeff(
p);
86 double Ic, dIcx = 0., dIcy = 0.;
95 double *tempt =
new double[
nbParam];
102 for (
unsigned int point = 0; point <
templateSize; point++) {
113 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
125 for (
unsigned int it = 0; it <
nbParam; it++)
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));
143 moyIref = moyIref / Nbpoint;
145 moyd2Iref = moyd2Iref / Nbpoint;
146 moyIc = moyIc / Nbpoint;
148 double covarIref = 0, covarIc = 0;
156 for (
unsigned int point = 0; point <
templateSize; point++) {
169 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
182 for (
unsigned int it = 0; it <
nbParam; it++) {
183 tempt[it] =
dW[0][it] * dIcx +
dW[1][it] * dIcy;
186 double prodIc = (Ic - moyIc);
188 double d_Ixx = dIxx.
getValue(i2, j2);
189 double d_Iyy = dIyy.
getValue(i2, j2);
190 double d_Ixy = dIxy.
getValue(i2, j2);
192 for (
unsigned int it = 0; it <
nbParam; it++) {
193 for (
unsigned int jt = 0; jt <
nbParam; jt++) {
194 sIcd2Iref[it][jt] += prodIc * (
dW[0][it] * (
dW[0][jt] * d_Ixx +
dW[1][jt] * d_Ixy) +
195 dW[1][it] * (
dW[0][jt] * d_Ixy +
dW[1][jt] * d_Iyy) - moyd2Iref[it][jt]);
196 sdIrefdIref[it][jt] +=
201 for (
unsigned int it = 0; it <
nbParam; it++)
204 covarIref += (Iref - moyIref) * (Iref - moyIref);
205 covarIc += (Ic - moyIc) * (Ic - moyIc);
206 sIcIref += (Iref - moyIref) * (Ic - moyIc);
211 covarIref = sqrt(covarIref);
212 covarIc = sqrt(covarIc);
214 denom = covarIref * covarIc;
216 double NCC = sIcIref / denom;
218 dcovarIref = -sIcdIref / covarIref;
221 dNCC = (sIcdIref / denom - NCC * dcovarIref / covarIref);
223 d2covarIref = -(sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.
t()) / covarIref;
227 Hdesire = (sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.
t()) / denom;
241 unsigned int iteration = 0;
246 double evolRMS_init = 0;
247 double evolRMS_prec = 0;
248 double evolRMS_delta;
251 unsigned int Nbpoint = 0;
253 Warp->computeCoeff(
p);
256 for (
unsigned int point = 0; point <
templateSize; point++) {
267 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
281 moyIref = moyIref / Nbpoint;
282 moyIc = moyIc / Nbpoint;
284 double covarIref = 0, covarIc = 0;
290 for (
unsigned int point = 0; point <
templateSize; point++) {
301 if ((i2 >= 0) && (j2 >= 0) && (i2 < I.
getHeight() - 1) && (j2 < I.
getWidth() - 1)) {
309 double prod = (Ic - moyIc);
310 for (
unsigned int it = 0; it <
nbParam; it++)
312 for (
unsigned int it = 0; it <
nbParam; it++)
315 covarIref += (Iref - moyIref) * (Iref - moyIref);
316 covarIc += (Ic - moyIc) * (Ic - moyIc);
317 sIcIref += (Iref - moyIref) * (Ic - moyIc);
320 covarIref = sqrt(covarIref);
321 covarIc = sqrt(covarIc);
322 double denom = covarIref * covarIc;
324 if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
327 double NCC = sIcIref / denom;
329 dcovarIref = sIrefdIref / covarIref;
330 G = (sIcdIref / denom - NCC * dcovarIref / covarIref);
346 if (iteration == 0) {
351 evolRMS_delta = std::fabs(
evolRMS - evolRMS_prec);
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
static void getGradYGauss2D(const vpImage< ImageType > &I, vpImage< FilterType > &dIy, const FilterType *gaussianKernel, const FilterType *gaussianDerivativeKernel, unsigned int size)
static void filter(const vpImage< ImageType > &I, vpImage< FilterType > &If, const vpArray2D< FilterType > &M, bool convolve=false)
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 getGradY(const vpImage< unsigned char > &I, vpImage< FilterType > &dIy)
unsigned int getWidth() const
Type getValue(unsigned int i, unsigned int j) const
unsigned int getHeight() const
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
static void computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
virtual void getParamInverse(const vpColVector &p, vpColVector &p_inv) const =0
virtual void getdW0(const int &v, const int &u, const double &dv, const double &du, double *dIdW)=0
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
virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &p12) const =0
void initHessienDesired(const vpImage< unsigned char > &I)
void trackNoPyr(const vpImage< unsigned char > &I)
void initCompInverse(const vpImage< unsigned char > &I)
vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp)
vpMatrix HLMdesireInverse
void computeEvalRMS(const vpColVector &p)
unsigned int iterationMax
void initPosEvalRMS(const vpColVector &p)
vpTemplateTrackerPoint * ptTemplate
vpTemplateTrackerWarp * Warp
unsigned int templateSize