//
// helper.
int
YARPFlowTracker::GrowMask (const YARPImageOf<YarpPixelMono>& src, YARPImageOf<YarpPixelMono>& dest)
{
// block filter 7x7
const int w = segmentation_mask.GetWidth();
const int h = segmentation_mask.GetHeight();
unsigned char *ss = (unsigned char *)src.GetRawBuffer();
dest.Zero();
// save in +2,+2...
unsigned char *s[7];
for (int ll = 0; ll < 7; ll++)
s[ll] = ss+ll*w;
int accum = 0;
for (int i = 0; i < h-7; i++)
{
for (int j = 0; j < w-7; j++)
{
accum = 0;
for (int k = 0; k < 7; k++)
{
accum += s[0][k];
accum += s[1][k];
accum += s[2][k];
accum += s[3][k];
accum += s[4][k];
accum += s[5][k];
accum += s[6][k];
}
accum /= 49;
accum = (accum >= 64) ? 255 : 0;
dest (j+3, i+3) = accum;
s[0] ++;
s[1] ++;
s[2] ++;
s[3] ++;
s[4] ++;
s[5] ++;
s[6] ++;
}
ss += w;
s[0] = ss;
s[1] = s[0]+w;
s[2] = s[1]+w;
s[3] = s[2]+w;
s[4] = s[3]+w;
s[5] = s[4]+w;
s[6] = s[5]+w;
}
return 0;
}
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;
}
