void YARPLpFirstOrderFlow::Apply (const YARPImageOf<YarpPixelMono>& currImg, const YARPImageOf<YarpPixelMono>& mask)
{
A_flow=0;
b_flow=0;
// Image FP conversion and log-polar border handling
_char2float(currImg.GetIplPointer(), Temp_img); //ok tested
// Apply Smoothing Filter
_compute_smoothing(Temp_img); //ok tested
// Apply Gaussian Filter
// LATER: Use separable kernel!!!!!!!
iplConvolve2DFP(Smoothing_img, Temp_img, &Gauss, 1, IPL_SUM);
// Apply Temporal Derivative Filter
_compute_derT(Temp_img); //ok tested
// Apply X Derivative Filter
iplConvolve2DFP(Temp_img, DerX_img, &DerivX, 1, IPL_SUM); //ok tested
// Apply Y Derivative Filter
iplConvolve2DFP(Temp_img, DerY_img, &DerivY, 1, IPL_SUM); //ok tested
// passing image array pointers
float *p_dx, *p_dy, *p_dt;
char *p_lineDerX, *p_lineDerY, *p_lineDerT;
unsigned char *p_maskVal;
char *p_lineMask;
IplImage *p_mask;
p_mask = mask.GetIplPointer();
p_lineDerX = DerX_img->imageData;
p_lineDerY = DerY_img->imageData;
p_lineDerT = DerT_img->imageData;
p_lineMask = p_mask->imageData;
double rad_quad;
int c, r;
int item = 1;
int limCol = nEcc - YARP_Border;
double dx, dy, dt;
// Skipping upper border
p_lineDerX += (DerX_img->widthStep * YARP_Border);
p_lineDerY += (DerY_img->widthStep * YARP_Border);
p_lineDerT += (DerT_img->widthStep * YARP_Border);
for (r=0; r<nAng; r++)
{
p_dx = (float *)p_lineDerX;
p_dy = (float *)p_lineDerY;
p_dt = (float *)p_lineDerT;
//skipping left border
p_dx += (YARP_Border);
p_dy += (YARP_Border);
p_dt += (YARP_Border);
p_maskVal = (unsigned char *)p_lineMask;
for (c=0; c<limCol; c++)
{
if (((int)*p_maskVal) < YARP_FuseThr)
{
dx = (double)*p_dx;
dy = (double)*p_dy;
dt = (double)*p_dt;
rad_quad = sqrt(dx*dx+dy*dy);
// Modified by Pasa (|| instead of &&).
if ((rad_quad > YARP_Threshold) || (fabs(dt) > YARP_ThreshLow))
{
SetMatrix(dx,dy,dt,item,c,r);
item++;
}
} // *maskVal.
p_dx++;
p_dy++;
p_dt++;
p_maskVal++;
}
p_lineDerX += DerX_img->widthStep;
p_lineDerY += DerY_img->widthStep;
p_lineDerT += DerT_img->widthStep;
p_lineMask += p_mask->widthStep;
}
assert (item >= YARP_MinPoints);
if (item >= YARP_MinPoints) // at least six components
{
A_trans=A_flow.Transposed();
sqA=A_trans*A_flow;
sqB=A_trans*b_flow;
VisDMatrixLU(sqA,sqB,flowComp);
}
else
{
flowComp = 0;
}
flowPoints = item-1;
}
int
YARPFlowTracker::ComputeRotation (
YARPImageOf<YarpPixelMono>& mask,
int *vx,
int *vy,
int ox,
int oy,
CVisDVector& trsf,
int thr)
{
const int border = 1;
trsf = 0;
trsf(1) = 1;
YARPImageOf<YarpPixelMono> tmp;
tmp.Resize (mask.GetWidth(), mask.GetHeight());
tmp.Zero();
double avex = 0;
double avey = 0;
double average = 0;
int count = 0;
// compute average displacement.
for (int i = border; i < oy-border; i++)
for (int j = border; j < ox-border; j++)
{
if (vx[i*ox+j] <= OOVERFLOW &&
vy[i*ox+j] <= OOVERFLOW &&
mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0
&&
(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0)
)
{
avex += vx[i*ox+j];
avey += vy[i*ox+j];
average += sqrt(vx[i*ox+j]*vx[i*ox+j]+vy[i*ox+j]*vy[i*ox+j]);
count++;
}
}
if (count > 0)
{
avex /= count;
avey /= count;
average /= count;
}
//
if (count >= thr)
{
CVisDMatrix A (count * 2, 4);
CVisDMatrix At (4, count * 2);
CVisDMatrix sqA (4, 4);
CVisDVector sqB (4);
CVisDVector b (count * 2);
CVisDVector solution(4);
count = 1;
for (int i = border; i < oy-border; i++)
for (int j = border; j < ox-border; j++)
{
if (vx[i*ox+j] <= OOVERFLOW &&
vy[i*ox+j] <= OOVERFLOW &&
mask (j*BLOCKINC+BLOCKSIZE/2, i*BLOCKINC+BLOCKSIZE/2) != 0
&&
(fabs(vx[i*ox+j]) > 0 || fabs(vy[i*ox+j]) > 0)
)
{
A(count,1) = j*BLOCKINC+BLOCKSIZE/2;
A(count,2) = i*BLOCKINC+BLOCKSIZE/2;
A(count,3) = 1;
A(count,4) = 0;
b(count) = j*BLOCKINC+BLOCKSIZE/2+vx[i*ox+j];
count++;
A(count,1) = i*BLOCKINC+BLOCKSIZE/2;
A(count,2) = -(j*BLOCKINC+BLOCKSIZE/2);
A(count,3) = 0;
A(count,4) = 1;
b(count) = i*BLOCKINC+BLOCKSIZE/2+vy[i*ox+j];
count++;
}
}
// solve by LU.
At = A.Transposed ();
sqA = At * A;
sqB = At * b;
VisDMatrixLU (sqA, sqB, solution);
trsf = solution;
// apply tranformation to mask.
double& aa = solution(1);
double& bb = solution(2);
double& t1 = solution(3);
double& t2 = solution(4);
for (int i = 0; i < mask.GetHeight(); i++)
for (int j = 0; j < mask.GetWidth(); j++)
{
if (mask (j, i) != 0)
{
int dx = int(aa * j + bb * i + t1 +.5);
int dy = int(-bb * j + aa * i + t2 + .5);
tmp.SafePixel (dx, dy) = 255;
}
}
mask = tmp;
return 0;
}
else
return -2;
return -1;
}
