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

  void Read(const char *fname = NULL)
    {
      if (!active)
	{
	  printf("Catalog loading from %s...\n",
		 (fname!=NULL)?fname:ORIENT_SRC);
	  YARPImageOf<YarpPixelFloat> orient_pre;
	  YARPImageFile::Read((fname!=NULL)?fname:ORIENT_SRC,
			      orient_pre,YARPImageFile::FORMAT_NUMERIC);

	  orient.Resize(4,orient_pre.GetHeight());
	  for (int i=0; i<orient_pre.GetHeight(); i++)
	    {
	      for (int j=0; j<4; j++)
		{
		  orient(j,i) = orient_pre(j,i);
		}
	    }

	  printf("Catalog loaded\n");
	  active = 1;
	}
    }

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

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

int YARPImageFile::Read(const char *src, YARPGenericImage& dest, int format)
{
 if (format!=YARPImageFile::FORMAT_NUMERIC)
    {
      return ImageRead(dest,src);
    }
  int hh = 0, ww = 0;
  {
    ifstream fin(src);
    int blank = 1;
    int curr = 0;
    while (!fin.eof())
      {
	int ch = fin.get();
	//if (ch!='\n') printf("[%c]",ch);
	if (ch==' ' || ch == '\t' || ch == '\r' || ch == '\n' || fin.eof())
	  {
	    if (!blank)
	      {
		if (curr==0)
		  {
		    hh++;
		  }
		curr++;
		if (curr>ww)
		  {
		    ww = curr;
		    //printf("%d\n", ww);
		  }
	      }
	    blank = 1;
	    if (ch=='\n')
	      {
		curr = 0;
	      }
	  }
	else
	  {
	    blank = 0;
	  }
      }
  }
  //printf("yyy dim %d %d\n", hh, ww);
  YARPImageOf<YarpPixelFloat> flt;
  flt.Resize(ww,hh);
  hh = 0; ww = 0;
  {
    char buf[256];
    int idx = 0;
    ifstream fin(src);
    int blank = 1;
    int curr = 0;
    while (!fin.eof())
      {
	int ch = fin.get();
	if (ch==' ' || ch == '\t' || ch == '\r' || ch == '\n' || fin.eof())
	  {
	    if (!blank)
	      {
		if (curr==0)
		  {
		    hh++;
		  }
		curr++;
		if (curr>ww)
		  {
		    ww = curr;
		  }
		buf[idx] = '\0';
		flt(curr-1,hh-1) = float(atof(buf));
		idx = 0;
	      }
	    blank = 1;
	    if (ch=='\n')
	      {
		curr = 0;
	      }
	  }
	else
	  {
	    buf[idx] = ch;
	    idx++;
	    assert(idx<sizeof(buf));
	    blank = 0;
	  }
      }
  }
    
  dest.CastCopy(flt);

  return 0;

  //return ImageRead(dest,src);
  //return 0;
}

int main(int argc, char* argv[])
{
	
	int boardN = 0;
	int ret = 0;
	int rhoSize = 256;
	int thetaSize = 180;
	
	YARPImageOf<YarpPixelMono> hough_img;
	char c;

	printf("\n[eyeCalib] Setting up the sensor..");
	ret = _grabber.initialize(boardN, _sizeX, _sizeY);
	if (ret == YARP_FAIL)
	{
		printf("[eyeCalib] ERROR in _grabber.initialize(). Quitting.\n");
		exit (1);
	}

	initializeHead();

	printf("\n[eyeCalib] Allocating images and filter (%d x %d)...", _sizeX, _sizeY);
	for (int i=0; i<N_IMAGES; i++)
		_imgBuffer[i].Resize (_sizeX, _sizeY);
	_susanImg.Resize (_sizeX, _sizeY);
	_susanFlt.resize(_sizeX, _sizeY);
	printf("\n[eyeCalib] Allocating Hough Filter (theta %d, rho %d)..", thetaSize, rhoSize);
	_houghFlt.resize(thetaSize, rhoSize);
	char fileName[255];
	FILE *outFile = NULL;
	double orientation;
	double posVector[4];
	double vel;
	printf("\n[eyeCalib] Do you want to save results to file ? [Y/n] ");
	c = getch();
	if (c != 'n')
	{
		
		printf("\n[eyeCalib] File name ? ");
		scanf("%s", fileName);
		printf("[eyeCalib] Append data (Y/n)?");
		c = getch();
		if (c == 'n')
		{
			outFile = fopen(fileName,"w");
			fprintf(outFile, "velocity;J0;J1;J2;J3;orientation\n");
		}
		else
		{
			outFile = fopen(fileName,"a");
		}

		if (!outFile)
		{
			printf("[eyeCalib] ERROR opening the file %s. Saving aboorted.\n", fileName);
		}
	}
	printf("\n[eyeCalib] Interactive calibration or Load data form file? [I/l] ");
	c = getch();
	if ( c == 'l')
	{
		printf("\n[eyeCalib] File to load? ");
		scanf("%s", fileName);
		FILE *dataFile = NULL;
		dataFile = fopen(fileName,"r");
		if (dataFile == NULL)
		{
			printf("[eyeCalib] ERROR opening the file %s. Quitting.\n", fileName);
			releaseSensor();
			if (outFile != NULL)
				fclose(outFile);
			exit(1);
		}
		bool ret = false;
		while (readLine(dataFile, &vel, posVector) == true)
		{
			printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
			printf("\n[eyeCalib] Hit any key when Position has been reached..");
			moveHead(vel,posVector);
			c = getch();
			orientation = calibrate();
			printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation));
			if (outFile != NULL)
				fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation);
		}
		fclose(dataFile);
	}
	else
	{
		do
		{
			printf("\n[eyeCalib] Avaible Joints:\n\t0 - Left Pan\n\t1 - Left Tilt\n\t2 - Right Pan\n\t3 - Left Tilt");
			printf("\n[EyeCalib] Move to? [J0;J1;J2;J3] ");
			scanf("%lf;%lf;%lf;%lf", &(posVector[0]), &(posVector[1]), &(posVector[2]), &(posVector[3]));
			printf("\n[eyeCalib] Move with velocity? ");
			scanf("%lf", &vel);
			printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
			printf("\n[eyeCalib] Hit any key when Position has been reached..");
			moveHead(vel, posVector);
			c = getch();
//.........这里部分代码省略.........