C++ YARPImageOf::Refer方法代码示例

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;
//.........这里部分代码省略.........

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
//.........这里部分代码省略.........