示例#1
0
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;
}
示例#3
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();
    }
}