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

//
// only find object_no.
//
int YARPObjectContainer::Segment (int object_no, YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& xx, int& yy)
{
	if (!m_active)
	{
		printf ("YARPObjectContainer: need to update stats first\n");
		out.PeerCopy(scan);
		xx = yy = 0;
		return -1;
	}

	double x, y, quality;
	m_locator[object_no].BackProject (scan, m_backp[object_no]);

	double ex, ey;
	m_locator[object_no].GetExtent (ex, ey);

	ex *= SCALE;
	ey *= SCALE;

	bool valid = false;

	if (m_locator[object_no].Find (ex, ey, x, y, quality) >= 0)
	{
		double mean = 0, stddev = 0;
		const double THR = 4.0;		// it was 2.0
		m_locator[object_no].GetExpectancy (mean, stddev);

		valid = (fabs(quality - mean) < stddev * THR) ? true : false;

#ifdef _DEBUG
		printf ("object: %d location: %lf %lf q: %lf\n", object_no, x, y, quality);
#endif
	}

	if (valid)
	{
		YarpPixelBGR red;
		red.r = 255;
		red.g = red.b = 0;

		double betterx = x;
		double bettery = y;
		AddRectangle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));

		//AddCircleOutline (m_backp[object_no], red, int(max_x+.5), int(max_y+.5), 10);
		AddCircle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), 5);

		// return processed image.
		out.PeerCopy (m_backp[object_no]);

		xx = int (betterx + .5);
		yy = int (bettery + .5);
	}
	else
	{
		xx = yy = 0;
	}

	return (valid) ? 0 : -1;
}

void Filter(YARPImageOf<YarpPixelBGR>& src,
	    YARPImageOf<YarpPixelBGR>& dest)
{
  FiveBoxesInARow& boxes = out_data.Content();
  YARPImageOf<YarpPixelMono> mono;
  mono.CastCopy(src);
  dest.PeerCopy(src);
  trackers.Update(mono,dest,boxes);
  out_data.Write();
  /*
  static ImgTrackTool track;
  dest.PeerCopy(src);
  track.Apply(dest);
   */
}

void YARPComplexTrackerTool::apply (YARPImageOf<YarpPixelBGR>& src, YARPImageOf<YarpPixelBGR>& dest, const YVector& jnts)
{
	/// check whether it's a new target first.
	_lock.Wait ();
	bool _isnew = _new_target;
	_lock.Post ();

	/// timing stuff.
	const double now = YARPTime::GetTimeAsSeconds();

	///
	bool act_vector = false;
	
	/// print timing issues.
	_diff_total += now - _last_round;
	_diff_count ++;

	if (now - _last_reset > PRINT_TIME)
    {
		printf("Time between frames in ms is %g\n", 1000 * _diff_total / _diff_count);
		_diff_count = 0;
		_diff_total = 0;
		_last_reset = now;
    }
	_last_round = now;

	/// LATER: this might be the place to auto-reset the tracker in case that any timeout expired.
	///
	///
	if (now - _last_movement > 30)
	{
		setNewTarget (ISIZE/2, ISIZE/2);
	}

	/// a bit of copying.
	_mono.CastCopy(src);
	dest.PeerCopy(src);
	
	///
	/// deals with the new target.
	if (_isnew)
	{
		_prev.PeerCopy (_mono);
		
		_lock.Wait ();
		_px = _tx = _ex;
		_py = _ty = _ey;
		_new_target = false;
		_lock.Post ();

		printf("*** Got new target %d %d\n", _px, _py);
		_last_update = now;
	}

	_tracker.SetBlockSize (BLOCK_SIZE, BLOCK_SIZE);
	_tracker.SetSearchWindowSize (SEARCH_SIZE, SEARCH_SIZE);
	_sub_tracker.SetBlockSize (BLOCK_SIZE, BLOCK_SIZE);
	_sub_tracker.SetSearchWindowSize (SEARCH_SIZE, SEARCH_SIZE);
	
	_gaze.update (jnts);

	if (!_isnew)
	{
		/// not a new target set the estimated offset.
		int predx = 0, predy = 0;
		_gaze.intersectRay (YARPHeadKinematics::KIN_LEFT, _prevRay, predx, predy);

		///predx += ISIZE/2;
		///predy += ISIZE/2;


		///
		YarpPixelBGR green (0, 255, 0);
		AddCircleOutline (dest, green, predx, predy, 5);
		AddCircleOutline (dest, green, predx, predy, 4);

		_dgx = predx - _prev_gaze_x;
		_dgy = predy - _prev_gaze_y;

		///printf ("est vel: %lf %lf\n", _dgx, _dgy);

		_tracker.SetSearchWindowOffset ((int)(_dgx+0.5), (int)(_dgy+0.5));
		_sub_tracker.SetSearchWindowOffset ((int)(_dgx+0.5), (int)(_dgy+0.5));
	}
	else
	{
		_tracker.SetSearchWindowOffset (0, 0);
		_sub_tracker.SetSearchWindowOffset (0, 0);
	}

	_tx = _px; 
	_ty = _py;

	/// checks borders.
	if (_tx < BXDX) _tx = BXDX;
	if (_tx > ISIZE-1-BXDX) _tx = ISIZE-1-BXDX;
	if (_ty < BXDX) _ty = BXDX;
	if (_ty > ISIZE-1-BXDX) _ty = ISIZE-1-BXDX;

	/// actual tracking.
//.........这里部分代码省略.........

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

//
// only find object and orientation.
//
int YARPObjectContainer::FindSimple (YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out)
{
	if (!m_active)
	{
		printf ("YARPObjectContainer: need to update stats first\n");
		out.PeerCopy(scan);
		m_last_known_object = -1;
		m_orientation = 0;
		m_displacement = 0;
		return -1;
	}

	YarpPixelBGR red;
	red.r = 255;
	red.g = red.b = 0;

	double x, y, quality;
	double max_quality = 0; 
	int max_obj = -1;
	double max_x = 0, max_y = 0;

	const int NOBJ = m_canonical_stats->m_goodneurons;

	for (int obj = 0; obj < NOBJ; obj++)
	{
		m_locator[obj].BackProject (scan, m_backp[obj]);

		double ex, ey;
		m_locator[obj].GetExtent (ex, ey);

		ex *= SCALE;
		ey *= SCALE;

		if (m_locator[obj].Find (ex, ey, x, y, quality) >= 0)
		{
			double mean = 0, stddev = 0;
			const double THR = 2.0;
			m_locator[obj].GetExpectancy (mean, stddev);

			bool thr_cond = (fabs(quality - mean) < stddev * THR) ? true : false;

#ifdef _DEBUG
			printf ("object: %d location: %lf %lf q: %lf\n", obj, x, y, quality);
#endif
			if (quality > max_quality && thr_cond)
			{
				max_quality = quality;
				max_obj = obj;
				max_x = x;
				max_y = y;
			}
		}
	}

	m_last_known_object = max_obj;

	if (max_obj == -1)
	{
		printf ("I (COG) don't know about this object, if there's one at all!\n");
		out.PeerCopy(scan);
		m_orientation = 0;
		m_displacement = 0;
		return -1;
	}

	// if max_quality is not good enough then skip the following search.
	// if pointiness is not good enough then skip the following search.
#ifdef _DEBUG
	printf ("object is %d, improving position\n", max_obj);
#endif

	double ex, ey;
	m_locator[max_obj].GetExtent (ex, ey);
	ex *= SCALE;
	ey *= SCALE;

	// try to improve the position estimation.
	double betterx = 0, bettery = 0;
	m_locator[max_obj].ImprovedSearch (scan, ex, ey, max_x, max_y, betterx, bettery);

#ifdef _DEBUG
	printf ("object is %d, looking for orientation\n", max_obj);
#endif

	double angle = -1;
	double angle2 = -1;
	double ave = 0;
	double std = 0;
	m_locator[max_obj].GetNumberOfPoints (ave, std);

	// need to add a threshold on pointiness!
	YARPSearchRotation sr (50, 0.0);
	sr.Search (int(m_canonical_stats->m_neuron_numbers(max_obj+1)), *m_canonical, scan, betterx, bettery, ave, std, angle, angle2);
	
	// store for future use.
	m_orientation = angle;
	m_displacement = 0;
//.........这里部分代码省略.........

int YARPObjectContainer::Find (YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& bestaction)
{
	if (!m_active)
	{
		printf ("YARPObjectContainer: need to update stats first\n");
		bestaction = -1;
		out.PeerCopy(scan);
		m_last_known_object = -1;
		m_orientation = 0;
		m_displacement = 0;
		return -1;
	}

	YarpPixelBGR red;
	red.r = 255;
	red.g = red.b = 0;

	double x, y, quality;
	double max_quality = 0; 
	int max_obj = -1;
	double max_x = 0, max_y = 0;

	const int NOBJ = m_canonical_stats->m_goodneurons;

	for (int obj = 0; obj < NOBJ; obj++)
	{
		m_locator[obj].BackProject (scan, m_backp[obj]);

		double ex, ey;
		m_locator[obj].GetExtent (ex, ey);

		ex *= SCALE;
		ey *= SCALE;

		if (m_locator[obj].Find (ex, ey, x, y, quality) >= 0)
		{
			double mean = 0, stddev = 0;
			const double THR = 2.0;
			m_locator[obj].GetExpectancy (mean, stddev);

			bool thr_cond = (fabs(quality - mean) < stddev * THR) ? true : false;

			printf ("object: %d location: %lf %lf q: %lf\n", obj, x, y, quality);

			if (quality > max_quality && thr_cond)
			{
				max_quality = quality;
				max_obj = obj;
				max_x = x;
				max_y = y;
			}
		}
	}

	m_last_known_object = max_obj;

	if (max_obj == -1)
	{
		printf ("I (COG) don't know about this object, if there's one at all!\n");
		bestaction = -1;
		out.PeerCopy(scan);
		m_orientation = 0;
		m_displacement = 0;
		return -1;
	}

	// if max_quality is not good enough then skip the following search.
	// if pointiness is not good enough then skip the following search.
	printf ("object is %d, improving position\n", max_obj);

	double ex, ey;
	m_locator[max_obj].GetExtent (ex, ey);
	ex *= SCALE;
	ey *= SCALE;

	// try to improve the position estimation.
	double betterx = 0, bettery = 0;
	m_locator[max_obj].ImprovedSearch (scan, ex, ey, max_x, max_y, betterx, bettery);

	printf ("object is %d, looking for orientation\n", max_obj);

	double angle = -1;
	double angle2 = -1;
	double ave = 0;
	double std = 0;
	m_locator[max_obj].GetNumberOfPoints (ave, std);

	// need to add a threshold on pointiness!
	YARPSearchRotation sr (50, 0.0);
	sr.Search (int(m_canonical_stats->m_neuron_numbers(max_obj+1)), *m_canonical, scan, betterx, bettery, ave, std, angle, angle2);
	
	// store for future use.
	m_orientation = angle;
	m_displacement = 0;

	printf ("orientation: %lf\n", angle * radToDeg);
	int x1 = betterx + cos(angle) * 40;
	int y1 = bettery + sin(angle) * 40;
	int x2 = betterx - cos(angle) * 40;
	int y2 = bettery - sin(angle) * 40;
//.........这里部分代码省略.........