1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71
| int u, v;
// parcour T
int i, j;
for(i=0; i<omega.width; i++)
{
u = i + omega.x;
for(j=0; j<omega.height; j++)
{
v = j + omega.y;
// X =(u,v,1)
SET_VECTOR(X, u, v);
// Z=W*X
cvGEMM(W, X, 1, 0, 0, Z);
// (u2,v2) appart I.
float u2, v2;
GET_VECTOR(Z, u2, v2);
int u2i = cvFloor(u2);
int v2i = cvFloor(v2);
if(u2i>=0 && u2i<pImgI->width && // si pixel appart I.
v2i>=0 && v2i<pImgI->height)
{
pixel_count++;
//gradient warpé
// interpolation.
float Ix = interpolate<short>(pGradIx, u2, v2);
float Iy = interpolate<short>(pGradIy, u2, v2);
// steepest descent
float stdesc[3];
stdesc[0] = (float)(-v*Ix+u*Iy);
stdesc[1] = (float)Ix;
stdesc[2] = (float)Iy;
//intensité par interpolation.
float I2 = interpolate<uchar>(pImgI, u2, v2);
// difference D = T(x)-I(W(x,p)).
float D = CV_IMAGE_ELEM(pImgT, uchar, v, u) - I2;
// mise à jour
mean_error += fabs(D);
// ajouter à matr
float* pb = &CV_MAT_ELEM(*b, float, 0, 0);
pb[0] += stdesc[0] * D;
pb[1] += stdesc[1] * D;
pb[2] += stdesc[2] * D;
// ajouter à Hess.
int l,m;
for(l=0;l<3;l++)
{
for(m=0;m<3;m++)
{
CV_MAT_ELEM(*H, float, l, m) += stdesc[l]*stdesc[m];
}
}
}
}
} |