int
YARPFlowTracker::CenterOfMass (YARPImageOf<YarpPixelMono>& in, int& x, int& y)
{
double xx = 0, yy = 0;
int count = 0;
for (int i = 0; i < in.GetHeight(); i++)
for (int j = 0; j < in.GetWidth(); j++)
{
if (in(j, i) != 0)
{
xx += j;
yy += i;
count ++;
}
}
if (count != 0)
{
x = int(xx / count + .5);
y = int(yy / count + .5);
}
else
{
x = in.GetWidth()/2;
y = in.GetHeight()/2;
}
return 0;
}
/// out -> 256 x 256, in 152 x 256.
///
int YARPLogpolar::Logpolar2Cartesian (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
using namespace _logpolarParams;
ACE_ASSERT (in.GetWidth() == _stheta && in.GetHeight() == _srho);
ACE_ASSERT (out.GetWidth() == _xsize && out.GetHeight() == _ysize);
Remap ((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _remapMap);
return YARP_OK;
}
void YARPBlobFinder::Apply(const YARPImageOf<YarpPixelRGB>& is, YARPImageOf<YarpPixelRGB>& id)
{
// no padding enforced.
assert (id.GetPadding() == 0);
assert (is.GetPadding() == 0);
assert (id.GetHeight() == is.GetHeight() && id.GetWidth() == is.GetWidth());
// extract the saliency image.
m_saliency.Apply (is, m_hsv_enhanced);
_apply (is, id);
}
bool FindCentroid (YARPImageOf<YarpPixelMono>& img, int *x, int *y)
{
char *ptri = img.GetRawBuffer ();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
unsigned char max = 0;
unsigned char *tmp = (unsigned char *)ptri;
for (int i = 0; i < w * h; i++, tmp++)
if (*tmp > max)
{
max = *tmp;
}
int count = 0;
*x = 0;
*y = 0;
for (i = 0; i < h; i++)
{
unsigned char *row = (unsigned char *)ptri + i * w;
for (int j = 0; j < w; j++, row++)
{
if (*row == max)
{
*x += j;
*y += i;
count ++;
}
}
}
if (count != 0)
{
*x /= count;
*y /= count;
}
else
{
*x = img.GetWidth() / 2;
*y = img.GetHeight() / 2;
}
if (max > 0)
return true;
else
return false;
}
void YARPImageTrackTool::Apply(YARPImageOf<YarpPixelBGR>& src)
{
YARPImageOf<YarpPixelBGR> nextImg, blah;
int ox, oy;
if (src.GetWidth() != IMG_W || src.GetHeight() != IMG_H)
{
printf("Image tracking code is old, and specific to %dx%d images\n", IMG_W, IMG_H);
exit(1);
}
nextImg.Refer(src);
if (first)
{
first = 0;
ResetXY();
prevImg3.PeerCopy(nextImg);
delta = 0;
}
else
{
ox = tx;
oy = ty;
ImgInt3& ii1 = *((ImgInt3 *)prevImg3.GetRawBuffer());
ImgInt3& ii2 = *((ImgInt3 *)src.GetRawBuffer());
Apply(ii1, ii2, tx, ty);
if (ox != tx || oy != ty || delta)
{
prevImg3.PeerCopy(nextImg);
delta = 0;
}
}
}
void YARPLpHistoSegmentation::Apply(YARPImageOf<YarpPixelHSV> &src)
{
int i;
int j;
unsigned char *h;
unsigned char *s;
unsigned char *v;
for(i = 0; i < src.GetHeight(); i++)
{
h = (unsigned char *) src.GetArray()[i];
s = h+1;
v = h+2;
double w = pSize(1, i);
for(j = 0; j < src.GetWidth(); j++)
{
if (_checkThresholds(*h,*s,*v))
YARP3DHistogram::Apply(*h, *s, 0, w);
h += 3;
s += 3;
v += 3;
}
}
}
YARPOrientation::YARPOrientation(const YARPImageOf<YarpPixelMono> &image)
{
nAng = image.GetHeight() ;
nEcc = image.GetWidth() ;
rfMin = 0.31 ;
initialise() ;
}
void YARPHistoSegmentation::Apply(YARPImageOf<YarpPixelBGR> &src)
{
int i;
int j;
unsigned char *r;
unsigned char *g;
unsigned char *b;
for(i = 0; i < src.GetHeight(); i++)
{
b = (unsigned char *) src.GetArray()[i];
g = b+1;
r = b+2;
// unsigned char rp, gp, bp;
for(j = 0; j < src.GetWidth(); j++)
{
// _normalize(*r, *g, *b, &rp, &gp, &bp);
// YARP3DHistogram::Apply(rp, gp, bp);
if (_checkThresholds(YarpPixelRGB(*r,*g,*b)))
YARP3DHistogram::Apply(*r, *g, *b);
b += 3;
g += 3;
r += 3;
}
}
}
void YARPImageUtils::PasteInto (const YARPImageOf<YarpPixelMono>& src, int x, int y, int zoom, YARPImageOf<YarpPixelMono>& dst)
{
char *bs = dst.GetRawBuffer ();
IplImage *ipl = src.GetIplPointer ();
const int dh = ipl->height;
const int dw = ipl->width;
char *dsY = ipl->imageData;
int depth = dst.GetPixelSize ();
ACE_ASSERT (depth == ipl->nChannels); // same # of chan.
const int h = dst.GetHeight();
ACE_ASSERT (h >= dh); // same height.
const int w = dst.GetWidth();
ACE_ASSERT (w >= dw); // same width.
const int rem_w = w - dw;
// crude limit check.
ACE_ASSERT (dw * zoom + x < w);
ACE_ASSERT (dh * zoom + y < h);
if (zoom == 1)
{
bs += (y * w);
for (int i = 0; i < dh; i++)
{
memcpy (bs + x, dsY, dw);
bs += w;
dsY += dw;
}
}
else
{
bs += (y * w);
for (int i = 0; i < dh; i++)
{
char * st_row = bs;
bs += x;
for (int j = 0; j < dw; j++)
{
for (int k = 0; k < zoom; k++)
{
*bs++ = *dsY;
}
dsY++;
}
for (int k = 1; k < zoom; k++)
memcpy (st_row + x + w * k, st_row + x, dw * zoom);
bs = st_row + w * zoom;
}
}
}
void YARPImageTrackTool::Apply(YARPImageOf<YarpPixelMono>& src, YARPImageOf<YarpPixelMono>& dest, int x, int y)
{
YARPImageOf<YarpPixelMono> prevImg, nextImg;
if ((src.GetWidth()!=IMG_W||src.GetHeight()!=IMG_H) ||
(dest.GetWidth()!=IMG_W||dest.GetHeight()!=IMG_H))
{
printf("Image tracking code is old, and specific to %dx%d images\n", IMG_W, IMG_H);
exit(1);
}
prevImg.Refer(src);
nextImg.Refer(dest);
tx = x; ty = y;
ImgInt& ii1 = *((ImgInt *)prevImg.GetRawBuffer());
ImgInt& ii2 = *((ImgInt *)nextImg.GetRawBuffer());
Apply(ii1, ii2, tx, ty);
}
void YARPColorConverter::RGB2Normalized (const YARPImageOf<YarpPixelRGB>& in, YARPImageOf<YarpPixelRGBFloat>& out, float threshold)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
assert (out.GetHeight() == in.GetHeight());
assert (out.GetWidth() == in.GetWidth());
unsigned char *inTmp = (unsigned char *) in.GetAllocatedArray();
unsigned char *outTmp = (unsigned char *) out.GetAllocatedArray();
int r = 0;
int c = 0;
int padIn = in.GetPadding();
int padOut = out.GetPadding();
float lum;
float *tmp;
for(r = 0; r<in.GetHeight(); r++)
{
for(c = 0; c < in.GetWidth(); c++)
{
tmp = (float *) outTmp;
lum = (float)( inTmp[0] + inTmp[1] + inTmp[2]);
if (lum > threshold)
{
tmp[0] = inTmp[0]/lum;
tmp[1] = inTmp[1]/lum;
tmp[2] = inTmp[2]/lum;
}
else
{
tmp[0] = 0.0;
tmp[1] = 0.0;
tmp[2] = 0.0;
}
inTmp += 3;
outTmp += 3*sizeof(float);
}
inTmp += padIn;
outTmp += padOut;
}
}
// this function now is exactly like the RGB one; however, in the future we may want to use
// different weights for different colors.
void YARPColorConverter::RGB2Normalized (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out, float threshold)
{
assert (out.GetIplPointer() != NULL && in.GetIplPointer() != NULL);
assert (out.GetHeight() == in.GetHeight());
assert (out.GetWidth() == in.GetWidth());
unsigned char *inTmp = (unsigned char *) in.GetAllocatedArray();
unsigned char *outTmp = (unsigned char *) out.GetAllocatedArray();
int r = 0;
int c = 0;
int padIn = in.GetPadding();
int padOut = out.GetPadding();
float lum;
for(r = 0; r<in.GetHeight(); r++)
{
for(c = 0; c < in.GetWidth(); c++)
{
lum = (float) (inTmp[0] + inTmp[1] + inTmp[2]);
if (lum > threshold)
{
outTmp[0] = (unsigned char)((inTmp[0]/lum)*255 + 0.5); // B
outTmp[1] = (unsigned char)((inTmp[1]/lum)*255 + 0.5); // G
outTmp[2] = (unsigned char)((inTmp[2]/lum)*255 + 0.5); // R
}
else
{
outTmp[0] = 0;
outTmp[1] = 0;
outTmp[2] = 0;
}
inTmp += 3;
outTmp += 3;
}
inTmp += padIn;
outTmp += padOut;
}
}
int YARPOrientation::Resize(const YARPImageOf<YarpPixelMono> &image)
{
cleanup() ;
nEcc = image.GetWidth() ;
nAng = image.GetHeight() ;
initialise() ;
return true ;
}
int YARPLogpolar::Logpolar2CartesianFovea (const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
using namespace _logpolarParams;
ACE_ASSERT (in.GetWidth() == _stheta && in.GetHeight() == _srho);
ACE_ASSERT (out.GetWidth() == _xsizefovea && out.GetHeight() == _ysizefovea);
_img.Size_X_Remap = _xsizefovea;
_img.Size_Y_Remap = _ysizefovea;
_img.Size_Img_Remap = _xsizefovea * _ysizefovea;
_img.Zoom_Level = 512.0/1090.0;
Remap ((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _remapMapFovea);
_img.Size_X_Remap = _xsize;
_img.Size_Y_Remap = _ysize;
_img.Size_Img_Remap = _xsize * _ysize;
_img.Zoom_Level = 256.0/1090.0;
return YARP_OK;
}
void SetPlane (const YARPImageOf<YarpPixelMono>& in, YARPGenericImage& out, int shift)
{
ACE_ASSERT (in.GetIplPointer() != NULL && out.GetIplPointer() != NULL);
ACE_ASSERT (in.GetWidth() == out.GetWidth());
ACE_ASSERT (in.GetHeight() == out.GetHeight());
const int width = in.GetWidth();
const int height = in.GetHeight();
unsigned char *src = NULL;
unsigned char *dst = NULL;
for (int i = 0; i < height; i++)
{
src = (unsigned char *)in.GetArray()[i];
dst = (unsigned char *)out.GetArray()[i] + shift;
for (int j = 0; j < width; j++)
{
*dst = *src++;
dst += 3;
}
}
}
void TmpFilter (YARPImageOf<YarpPixelMono>& old, YARPImageOf<YarpPixelMono>& img)
{
char *src = img.GetRawBuffer();
char *optr = old.GetRawBuffer();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
for (int i = 0; i < w * h; i++)
{
*optr = 0.5 * *src++ + 0.5 * *optr;
optr++;
}
memcpy (img.GetRawBuffer(), old.GetRawBuffer(), w * h);
}
// very very stupid hash generator.
NetInt32 YARPImageHash::GetHash(YARPImageOf<YarpPixelBGR>& src)
{
NetInt32 key = 0;
int y = src.GetHeight()/2;
int ent = 0;
for (int x=src.GetWidth()-10; x>=10 && ent<5; x--)
{
YarpPixelBGR& pix = src.SafePixel(x,y);
if ((pix.r>=10 && pix.r<=200) || x<=15)
{
key *= 17;
key += pix.r;
key *= 17;
key += pix.g;
key *= 17;
key += pix.b;
ent++;
}
}
return key;
}
void YARPHistoSegmentation::backProjection(YARPImageOf<YarpPixelHSV> &in, YARPImageOf<YarpPixelMono> &out)
{
// complete histogram backprojection
int i;
int j;
YarpPixelHSV *src;
YarpPixelMono *dst;
for(j = 0; j < in.GetHeight(); j++)
{
src = (YarpPixelHSV *) in.GetArray()[j];
dst = (YarpPixelMono *) out.GetArray()[j];
for(i = 0; i < in.GetWidth(); i++)
{
if (_checkThresholds(src->h, src->s, src->v))
*dst = (unsigned char)(YARP3DHistogram::backProjection(src->h, src->s, 0)*255 + 0.5);
else
*dst = 0;
src++;
dst++;
}
}
}
void YARPHistoSegmentation::backProjection(YARPImageOf<YarpPixelBGR> &in, YARPImageOf<YarpPixelMono> &out)
{
// complete histogram backprojection
int i;
int j;
YarpPixelBGR *src;
YarpPixelRGB tmp;
YarpPixelMono *dst;
for(j = 0; j < in.GetHeight(); j++)
{
src = (YarpPixelBGR *) in.GetArray()[j];
dst = (YarpPixelMono *) out.GetArray()[j];
for(i = 0; i < in.GetWidth(); i++)
{
// _normalize(*src, tmp);
if (_checkThresholds(YarpPixelRGB(src->r, src->g, src->b)))
*dst = (unsigned char)(YARP3DHistogram::backProjection(*src)*255 + 0.5);
else
*dst = 0;
src++;
dst++;
}
}
}
int YARPImageFile::Write(const char *dest, YARPGenericImage& src, int format)
{
if (format!=YARPImageFile::FORMAT_NUMERIC)
{
return ImageWrite(src,dest);
}
YARPImageOf<YarpPixelFloat> flt;
ofstream fout(dest);
flt.CastCopy(src);
for (int i=0; i<flt.GetHeight(); i++)
{
for (int j=0; j<flt.GetWidth(); j++)
{
char buf[255];
sprintf(buf,"%g ", flt(j,i));
fout << buf << " ";
}
fout << endl;
}
return 0;
//return ImageWrite(src,dest);
//return 0;
}
int
YARPFlowTracker::WriteMask (YARPImageOf<YarpPixelMono>& mask, YARPImageOf<YarpPixelBGR>& out, YarpPixelBGR *color)
{
YarpPixelBGR c;
if (color == NULL)
{
c.r = 255;
c.g = 0;
c.b = 255;
}
else
c = *color;
for (int j = 0; j < mask.GetHeight(); j++)
for (int i = 0; i < mask.GetWidth(); i++)
if (mask(i,j) != 0)
{
//out(i,j) = c;
//out(i,j).r = c.r;
out(i,j).b = c.b;
}
return 0;
}
void DrawBoxes (FiveBoxesInARow boxes, YARPImageOf<YarpPixelMono>& img, int weight, int scalefactor)
{
char *ptri = img.GetRawBuffer ();
const int w = img.GetWidth ();
const int h = img.GetHeight ();
// weird stuff...
//
CBox2Send *array = &(boxes.box1);
for (int i = 0; i < 5; i++)
{
CBox2Send *tmp = array++;
if (tmp->valid && Area (tmp) > 500)
{
// draw box i-th
int minr = (tmp->ymin < 0) ? 0 : tmp->ymin;
int maxr = (tmp->ymax >= h) ? h-1 : tmp->ymax;
int minc = (tmp->xmin < 0) ? 0 : tmp->xmin;
int maxc = (tmp->xmax >= w) ? w-1 : tmp->xmax;
for (int r = minr; r <= maxr; r++)
{
char *row = ptri + r * w + tmp->xmin;
for (int c = minc; c <= maxc; c++)
{
int result = *row + weight * scalefactor;
if (result >= 255)
*row++ = 255;
else
*row++ = result;
}
}
}
}
}
int YARPImageLabel::ApplySimilarity(YARPImageOf<YarpPixelInt>& src,
YARPImageOf<YarpPixelInt>& dest)
{
int IMG_H = src.GetWidth(); // swapped role of H and W
int IMG_W = src.GetHeight();
int nalloc_len = IMG_H*IMG_W*4;
if (nalloc_len>alloc_len)
{
Clean();
alloc_len = nalloc_len;
xstack = new int [alloc_len];
ystack = new int [alloc_len];
}
assert(xstack!=NULL&&ystack!=NULL);
int i, j;
int ii, jj;
int id = 1;
int top = 0;
int maxCount = 0;
int maxCountId = 0;
int count;
for (i=0; i<IMG_H; i++)
{
for (j=0; j<IMG_W; j++)
{
dest(i,j) = 0;
}
}
for (i=0; i<IMG_H; i++)
{
for (j=0; j<IMG_W; j++)
{
if (dest(i,j) == 0)
{
int ref = src(i,j);
id++;
//if (id>255)
//{
// id = 2;
//}
count = 0;
top = 1;
ystack[0] = i;
xstack[0] = j;
while (top>0)
{
top--;
ii = ystack[top];
jj = xstack[top];
if (dest(ii,jj) == 0 && src(ii,jj)==ref)
{
dest(ii,jj) = id;
count++;
if (ii>0)
{
if (dest(ii-1,jj) == 0 && src(ii-1,jj)==ref)
{
ystack[top] = ii-1;
xstack[top] = jj;
top++;
}
}
if (ii+1<IMG_H)
{
if (dest(ii+1,jj) == 0 && src(ii+1,jj)==ref)
{
ystack[top] = ii+1;
xstack[top] = jj;
top++;
}
}
if (jj>0)
{
if (dest(ii,jj-1) == 0 && src(ii,jj-1)==ref)
{
ystack[top] = ii;
xstack[top] = jj-1;
top++;
}
}
if (jj+1<IMG_W)
{
if (dest(ii,jj+1) == 0 && src(ii,jj+1)==ref)
{
ystack[top] = ii;
xstack[top] = jj+1;
top++;
}
}
}
}
if (count>maxCount)
{
maxCount = count;
maxCountId = id;
}
}
}
}
bestId = maxCountId;
return maxCountId;
}
int main_alt()
{
in_head.Register("/egomap/i:head");
in_img.Register("/egomap/i:img");
out_img.Register("/egomap/o:img");
out_cmd.Register("/egomap/o:cmd");
in_voice.Register("/egomap/i:cmd");
while (1)
{
JointPos joints;
in_img.Read();
state_mutex.Wait();
joints = state_joint;
CogGaze gaze;
gaze.Apply(joints);
double roll = gaze.roll_right;
double theta = gaze.theta_right;
double phi = gaze.phi_right;
//printf("DIR %g %g %g\n", theta, phi, roll);
global_theta = theta;
global_phi = phi;
global_roll = roll;
state_mutex.Post();
double z_x = gaze.z_right[0];
double z_y = gaze.z_right[1];
YARPImageOf<YarpPixelBGR> img;
img.Refer(in_img.Content());
int width = img.GetWidth();
int height = img.GetHeight();
float s = 50;
for (int i=0; i<width; i++)
{
YarpPixelBGR pix0(0,255,0);
img(i,width/2) = pix0;
}
for (int i=0; i<width; i++)
{
float s2 = (i-width/2.0);
float x = cos(roll)*s2;
float y = -sin(roll)*s2;
YarpPixelBGR pix(255,0,0);
img.SafePixel((int)(0.5+x+(width+1)/2.0),(int)(0.5+y+(width+1)/2.0)) = pix;
}
int step = 500;
for (int i=0; i<step; i++)
{
float theta = i*M_PI*2.0/step;
YarpPixelBGR pix(255,0,0);
float x = cos(theta)*s;
float y = sin(theta)*s;
//printf("%g %g %g\n", theta, x, y);
img.SafePixel(x+width/2,y+width/2) = pix;
}
for (int i=0; i<MAX_TARGETS; i++)
{
if (target_manager.Exists(i))
{
TargetLocation& loc = target_manager.Get(i);
float target_theta = loc.theta;
float target_phi = loc.phi;
float z_y = loc.phi/(M_PI/2);
float z_x = loc.theta/(M_PI/2);
//printf("Drawing circle for %g %g\n", loc.theta, loc.phi);
float x = z_x*s;
float y = z_y*s;
// YarpPixelBGR pix0(0,128,255);
// AddCircle(img,pix0,(int)x+width/2,(int)y+height/2,4);
// We now try to map back
// onto approximate retinotopic coordinates.
// current x, y, z available in gaze::x_right,y_right,z_right
double x_vis, y_vis;
int visible;
visible = gaze.Intersect(target_theta,target_phi,x_vis,y_vis,
CAMERA_SOURCE_RIGHT_WIDE);
/*
float zt[3];
zt[0] = sin(target_theta);
zt[1] = -cos(target_phi)*cos(target_theta);
zt[2] = sin(target_phi)*cos(target_theta);
float delta_theta = zt[0]*gaze.x_right[0] +
zt[1]*gaze.x_right[1] + zt[2]*gaze.x_right[2];
float delta_phi = zt[0]*gaze.y_right[0] +
zt[1]*gaze.y_right[1] + zt[2]*gaze.y_right[2];
float sanity = zt[0]*gaze.z_right[0] +
zt[1]*gaze.z_right[1] + zt[2]*gaze.z_right[2];
//float delta_theta = zt[0]; //target_theta - global_theta;
//float delta_phi = zt[1]; //target_phi - global_phi;
float factor_theta = 67; // just guessed these numbers
float factor_phi = 67; // so far, not linear in reality
float nx = delta_theta;
float ny = delta_phi;
float r = global_roll;
float sinr = sin(r);
float cosr = cos(r);
float fx = factor_theta;
float fy = factor_phi;
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
delta_theta *= factor_theta;
delta_phi *= factor_phi;
delta_phi *= 4.0/3.0;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += img.GetWidth()/2;
delta_phi += img.GetHeight()/2;
*/
int sanity = visible;
double delta_theta = x_vis;
double delta_phi = y_vis;
delta_theta -= 64;
delta_phi -= 64;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += 64;
delta_phi += 64;
if (sanity>0)
{
YarpPixelBGR pix1((len<50)?255:0,128,0);
AddCircle(img,pix1,(int)(delta_theta+0.5),
(int)(delta_phi+0.5),4);
}
else
{
//printf("Object occluded\n");
}
}
}
z_y = phi/(M_PI/2);
z_x = theta/(M_PI/2);
if (0)
for (int i=0; i<5; i++)
{
float x = z_x*s;
float y = z_y*s;
YarpPixelBGR pix(255,0,0);
YarpPixelBGR pix2(0,0,255);
img.SafePixel(i+x+width/2,y+width/2) = pix;
img.SafePixel(-i+x+width/2,y+width/2) = pix;
img.SafePixel(i+x+width/2,y+width/2-1) = pix2;
img.SafePixel(-i+x+width/2,y+width/2+1) = pix2;
img.SafePixel(x+width/2,i+y+width/2) = pix;
img.SafePixel(x+width/2,-i+y+width/2) = pix;
img.SafePixel(x+width/2+1,i+y+width/2) = pix2;
img.SafePixel(x+width/2-1,-i+y+width/2) = pix2;
}
out_img.Content().PeerCopy(in_img.Content());
out_img.Write();
}
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;
}
float Process(YARPImageOf<YarpPixelBGR>& ref,
YARPImageOf<YarpPixelBGR>& target)
{
static float bounce = 0;
static int hash_ref[HASH_SIZE];
static int hash_target[HASH_SIZE];
static int hash_target_ylo[HASH_SIZE];
static int hash_target_yhi[HASH_SIZE];
// printf("Clearing hash tables\n");
for (int r=0; r<HASH_SIZE; r++)
{
hash_ref[r] = hash_target[r] = 0;
hash_target_ylo[r] = hash_target_yhi[r] = 0;
}
// printf("Comparing images\n");
int h = ref.GetHeight();
int w = ref.GetWidth();
assert(h==target.GetHeight());
assert(w==target.GetWidth());
for (int y=0; y<h; y++)
{
for (int x=0; x<w; x++)
{
YarpPixelBGR pix_ref = ref(x,y);
YarpPixelBGR pix_target = target(x,y);
int idx_ref = Hash(pix_ref);
int idx_target = Hash(pix_target);
hash_ref[idx_ref]++;
hash_target[idx_target]++;
if (y>=h/2)
{
hash_target_yhi[idx_target]++;
}
else
{
hash_target_ylo[idx_target]++;
}
}
}
int votes = 0;
float dir = 0;
for (int r=0; r<HASH_SIZE; r++)
{
int diff = hash_ref[r] - hash_target[r];
if (fabs(diff)>3)
{
int hi = hash_target_yhi[r];
int lo = hash_target_ylo[r];
if (fabs(hi-lo)>3)
{
float power = fabs(hi-lo)*fabs(diff);
if (power>20) power = 20;
float direction = power;
if (diff>0)
{
if (hi<lo)
{
direction = -direction;
}
}
else
{
if (hi>lo)
{
direction = -direction;
}
}
votes++;
dir += direction;
}
}
}
printf("%8d votes, verdict is %8.4g ", votes, dir);
bounce = 0.8*bounce + 0.2*((dir>0)?1:-1);
if (bounce>0.5) printf("LOOK DOWN");
if (bounce<-0.5) printf("LOOK UP");
printf("\n");
return dir;
}
void Update(YARPImageOf<YarpPixelMono>& img,
YARPImageOf<YarpPixelBGR>& dest,
FiveBoxesInARow& out_boxes)
{
if (first)
{
prev.PeerCopy(img);
first = 0;
}
int count = 0;
int count2 = 0;
int index = 0;
mutex.Wait();
UpdateActivity();
int box_index = 0;
for (int k=0; k<FiveBoxesInARow::GetMaxBoxes(); k++)
{
out_boxes(k).valid = false;
}
for (int i=0; i<MAX_TRACKER; i++)
{
if (tracker[i].is_active)
{
count2++;
}
if (tracker[i].is_tracking)
{
count++;
int ox = tracker[i].box.cx;
int oy = tracker[i].box.cy;
int x = ox;
int y = oy;
int theta = 15;
if (oy>theta && oy<=img.GetHeight()-theta &&
ox>theta && ox<=img.GetWidth()-theta)
{
if (tracker[i].is_lagged)
{
track_tool.Apply(face_prev,img,ox,oy);
tracker[i].is_lagged = 0;
}
else
{
track_tool.Apply(prev,img,ox,oy);
}
x = track_tool.GetX();
y = track_tool.GetY();
}
int dx = x-ox;
int dy = y-oy;
if (dx!=0 || dy!=0)
{
// printf("Delta %d %d (to %d %d)\n", dx, dy, x, y);
}
tracker[i].box.brx += dx;
tracker[i].box.bry += dy;
tracker[i].box.tlx += dx;
tracker[i].box.tly += dy;
tracker[i].box.cx += dx;
tracker[i].box.cy += dy;
if (index<FiveBoxesInARow::GetMaxBoxes())
{
CBox2Send& dest2 = out_boxes(box_index);
box_index++;
Box& src = tracker[i].box;
dest2.xmin = src.tlx;
dest2.ymin = src.tly;
dest2.xmax = src.brx;
dest2.ymax = src.bry;
dest2.valid = true;
for (int i = -3; i<= 3; i++)
{
for (int j=-3; j<=3; j++)
{
if ((i+j)%2)
{
dest.SafePixel(x+j,y+i) = YarpPixelBGR(255,255,255);
}
else
{
dest.SafePixel(x+j,y+i) = YarpPixelBGR(0,0,0);
}
}
}
}
}
}
mutex.Post();
//if (count>0)
{
// printf("*** %d trackers tracking, %d active\n", count,
// count2);
}
prev.PeerCopy(img);
}