void YARPLpConicFitter::plotCircle(int T0, int R0, double R, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int theta;
int rho;
double r0;
r0 = _moments.CsiToRo(R0);
for(theta = 0; theta < _logpolarParams::_stheta; theta++)
{
double c = cos((theta-T0)/_q);
double DELTA = (r0*r0*(c*c-1) + R*R);
if (DELTA>=0)
{
int r = (int) ((r0*c+sqrt(DELTA)) + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
r = (int) ((r0*c-sqrt(DELTA)) + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
}
}
// plot center
output.SafePixel(T0, R0) = v;
}
void YARPConicFitter::plotEllipse(int X0, int Y0, double a11, double a12, double a22, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int t;
float theta = 0.0;
const int nThetaS = 200;
const float deltaTh= (float) PI/ (double) nThetaS;
float x,y,r;
if ( !_checkDet(a11, a12, a22) )
{
// sorry, not an ellipse...
return;
}
for(t = 0; t < nThetaS; t++)
{
theta = deltaTh*t;
double c = cos(theta);
double s = sin(theta);
double A = a11*c*c+2*a12*c*s+a22*s*s;
if (A > 0)
{
r = sqrt(1/A);
int xx = int (r*c+0.5);
int yy = int (r*s+0.5);
x = X0 + xx;
y = Y0 - yy;
output.SafePixel(x,y) = v;
x = X0 - xx;
y = Y0 + yy;
output.SafePixel(x,y) = v;
}
}
// plot center
output.SafePixel(X0, Y0) = v;
}
void YARPLpConicFitter::plotEllipse(int T0, int R0, double a11, double a12, double a22, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
int theta;
int rho;
double r0;
if ( !_checkDet(a11, a12, a22) )
{
// sorry, not an ellipse...
return;
}
r0 = _moments.CsiToRo(R0);
double c0 = cos((T0)/_q);
double s0 = sin((T0)/_q);
for(theta = 0; theta < _logpolarParams::_stheta; theta++)
{
double c = cos((theta)/_q);
double s = sin((theta)/_q);
double A;
double B;
double C;
A = a11*c*c + 2*a12*c*s + a22*s*s;
B = r0*(a11*c*c0 + a12*c*s0 + a12*c0*s + a22*s*s0);
C = r0*r0*(2*a12*c0*s0 + a22*s0*s0 + a11*c0*c0)-1;
double DELTA = B*B-A*C;
if (DELTA >= 0)
{
int r = (int) ((B+sqrt(DELTA))/A + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
r = (int) ((B-sqrt(DELTA))/A + 0.5);
if (r > 0)
{
rho = _moments.RoToCsi(r);
if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
output(theta,rho) = v;
}
}
}
// plot center
output.SafePixel(T0, R0) = v;
}
// 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;
}
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;
}
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);
}
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;
}
