int main(int argc, const char *argv[])
{
const char *name = DEFAULT_NAME;
char buf[256];
if (argc>1)
{
name = argv[1];
}
sprintf(buf, "%s/i:img", name);
in_img.Register(buf);
sprintf(buf, "%s/o:img", name);
out_img.Register(buf);
sprintf(buf, "%s/i:mix", name);
in_mix.Register(buf);
sprintf(buf, "%s/o:box", name);
out_data.Register(buf);
box_thread.Begin();
while(1)
{
in_img.Read();
YARPImageOf<YarpPixelBGR> in;
YARPImageOf<YarpPixelBGR> out;
in.Refer(in_img.Content());
out_img.Content().SetID(YARP_PIXEL_BGR);
SatisfySize(in,out_img.Content());
out.Refer(out_img.Content());
Filter(in,out);
out_img.Write();
// YARPTime::DelayInSeconds(1000000);
}
return 0;
}
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 main(int argc, char *argv[])
{
ParseCommandLine (argc, argv);
YARPImageOf<YarpPixelBGR> workImgL; // just a reference.
YARPImageOf<YarpPixelBGR> workImgR; // just a reference.
YARPImageOf<YarpPixelMono> lpImg;
lpImg.Resize (Size, Size);
YARPImageOf<YarpPixelMono> cartesianl;
YARPImageOf<YarpPixelRGB> in_cartl;
YARPImageOf<YarpPixelRGB> logpolarl;
YARPImageOf<YarpPixelMono> m_storeleft;
YARPImageOf<YarpPixelMono> cartesianr;
YARPImageOf<YarpPixelRGB> in_cartr;
YARPImageOf<YarpPixelRGB> logpolarr;
YARPImageOf<YarpPixelMono> m_storeright;
cartesianl.Resize (Size, Size);
in_cartl.Resize (Size, Size);
logpolarl.Resize (NECC, NANG);
m_storeleft.Resize (NECC, NANG);
cartesianr.Resize (Size, Size);
in_cartr.Resize (Size, Size);
logpolarr.Resize (NECC, NANG);
m_storeright.Resize (NECC, NANG);
YARPImageOf<YarpPixelMono> m_back_histo;
m_back_histo.Resize (15, 10);
YARPImageOf<YarpPixelMono> m_histo;
m_histo.Resize (15, 10);
VisionThread2Status m_status = VTS_WaitForTarget;
int m_counter = 0;
YARPLogPolar lp (NECC, NANG, rfMin, Size);
YARPLpClrSegmentation m_color_segmentation (NECC, NANG, rfMin, Size);
YARPLpBoxer m_boxer (NECC, NANG, rfMin, Size);
FiveBoxesInARow target_boxes_l;
FiveBoxesInARow target_boxes_r;
inImgPortL.Register(in_channell);
inImgPortR.Register(in_channelr);
outImgPortL.Register(out_channell);
outImgPortR.Register(out_channelr);
histoPort.Register(histo_channel);
out_position_l.Register (position_channel_l);
out_position_r.Register (position_channel_r);
while (1)
{
inImgPortL.Read();
YARPGenericImage& inImgL = inImgPortL.Content(); // Lifetime until next Read()
workImgL.Refer(inImgL);
inImgPortR.Read();
YARPGenericImage& inImgR = inImgPortR.Content(); // Lifetime until next Read()
workImgR.Refer(inImgR);
lp.Cart2LpSwap (workImgL, logpolarl);
lp.Cart2LpSwap (workImgR, logpolarr);
switch (m_status)
{
case VTS_WaitForTarget:
if (m_color_segmentation.InitialDetection (logpolarl, logpolarr, m_storeleft, m_storeright))
m_status = VTS_ColorSegmentationInit;
break;
case VTS_ColorSegmentationInit:
{
if (m_color_segmentation.StartupSegmentation (logpolarl, logpolarr, m_storeleft, m_storeright))
{
m_status = VTS_ColorSegmentation;
m_counter = 0;
}
else
m_status = VTS_WaitForTarget;
}
break;
case VTS_ColorSegmentation:
{
// wait 6 steps before aborting in case of bad target...
bool can_continue = m_color_segmentation.ColorSegmentation (logpolarl, logpolarr, m_storeleft, m_storeright);
if (!can_continue)
{
// wait some cycles...
m_counter++;
if (m_counter > 20) // || !good)
{
// move to motion segmentation.
m_status = VTS_WaitForTarget;
m_color_segmentation.Reset ();
}
}
else
{
m_counter = 0;
}
}
break;
case VTS_ResetHistograms:
m_color_segmentation.ResetHistograms (); // continues aborting the segmentation.
case VTS_AbortSegmentation:
m_color_segmentation.AbortSegmentation ();
m_status = VTS_WaitForTarget;
break;
}
m_boxer.Apply (m_storeleft);
lp.Lp2Cart (m_storeleft, cartesianl);
m_boxer.DrawBoxes (cartesianl);
YARPBox *b = m_boxer.GetBoxes();
YARPBoxCopier::Copy (b[0], target_boxes_l.box1);
YARPBoxCopier::Copy (b[1], target_boxes_l.box2);
YARPBoxCopier::Copy (b[2], target_boxes_l.box3);
YARPBoxCopier::Copy (b[3], target_boxes_l.box4);
YARPBoxCopier::Copy (b[4], target_boxes_l.box5);
m_boxer.Apply (m_storeright);
lp.Lp2Cart (m_storeright, cartesianr);
m_boxer.DrawBoxes (cartesianr);
YARPBoxCopier::Copy (b[0], target_boxes_r.box1);
YARPBoxCopier::Copy (b[1], target_boxes_r.box2);
YARPBoxCopier::Copy (b[2], target_boxes_r.box3);
YARPBoxCopier::Copy (b[3], target_boxes_r.box4);
YARPBoxCopier::Copy (b[4], target_boxes_r.box5);
out_position_l.Content() = target_boxes_l;
out_position_l.Write();
out_position_r.Content() = target_boxes_r;
out_position_r.Write();
m_color_segmentation.GetMotionHistogramAsImage(m_histo);
m_color_segmentation.GetWholeHistogramAsImage(m_back_histo);
YARPImageUtils::PasteInto (m_histo, 0, 0, 5, lpImg);
YARPImageUtils::PasteInto (m_back_histo, 0, 60, 5, lpImg);
YARPGenericImage& outImgL = outImgPortL.Content(); // Lasts until next Write()
outImgL.Refer(cartesianl);
outImgPortL.Write();
YARPGenericImage& outImgR = outImgPortR.Content(); // Lasts until next Write()
outImgR.Refer(cartesianr);
outImgPortR.Write();
YARPGenericImage& outhImg = histoPort.Content();
outhImg.Refer(lpImg);
histoPort.Write();
}
}
