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

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;

}

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

  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 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;
}