//
// 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);
   */
}
Beispiel #3
0
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.
	bool fell = false;
	double new_tx = ISIZE/2, new_ty = ISIZE/2, new_tx2 = ISIZE/2, new_ty2 = ISIZE/2;
	int sub_x = ISIZE/2, sub_y = ISIZE/2, sub_x2 = ISIZE/2, sub_y2 = ISIZE/2;
	
	_tracker.Apply (_prev, _mono, _tx, _ty);

	int x = _tx, y = _ty;
	x = _tracker.GetX();
	y = _tracker.GetY();

	/// predicted.
	double new_dx = x - (_tx + _dgx);
	double new_dy = y - (_ty + _dgy);

	/// direction of motion.
	double new_mag = sqrt (new_dx * new_dx + new_dy * new_dy);
	if (new_mag < 0.001) new_mag = 0.001;
	new_dx /= new_mag;
	new_dy /= new_mag;

	const double nscale = NSCALE;

	/// search along two directions.
	/// heuristic for exploring certain neighborhood of the current position.
	///
	///
	new_tx = _tx - new_dx * nscale;
	new_ty = _ty - new_dy * nscale;
	new_tx2 = _tx + new_dx * nscale;
	new_ty2 = _ty + new_dy * nscale;

	_sub_tracker.Apply (_prev, _mono, new_tx2, new_ty2);

	sub_x2 = _sub_tracker.GetX();
	sub_y2 = _sub_tracker.GetY();

	_sub_tracker.Apply (_prev, _mono, new_tx, new_ty);

	sub_x = _sub_tracker.GetX();
	sub_y = _sub_tracker.GetY();
	
	double sub_dx = sub_x - (new_tx + _dgx);
	double sub_dy = sub_y - (new_ty + _dgy);
	double sub_mag = sqrt (sub_dx * sub_dx + sub_dy * sub_dy);

	double sub_dx2 = sub_x2 - (new_tx2 + _dgx);
	double sub_dy2 = sub_y2 - (new_ty2 + _dgy);
	double sub_mag2 = sqrt (sub_dx2 * sub_dx2 + sub_dy2 * sub_dy2);

	if (new_mag > MAGDEFAULT)
	{
		act_vector = true;

		if (sub_mag > MAGDEFAULT && sub_mag2 < MAGDEFAULT)
		{
			printf("Should fall inwards\n");
			x = (int)sub_x;
			y = (int)sub_y;
			fell = true;
		}

		if (sub_mag2 > MAGDEFAULT && sub_mag < MAGDEFAULT)
		{
			printf("Should fall outwards\n");
			x = (int)sub_x2;
			y = (int)sub_y2;
			fell = true;
		}
	}

	_tx = x;
	_ty = y;

	float quality = _tracker.GetQuality();
	bool low_quality = false;

	if (quality < QTHRESHOLD)
	{
		///printf("low match quality (%g)\n", quality);

		if (_low_q_ct < QTHR2)
		{
			_low_q_ct++;
		}

		/// things are not going well.
		if (_low_q_ct > QTHR3)
		{
			low_quality = true;
			x = _tx = _px + _dgx;
			y = _ty = _py + _dgy;
		}
	}
	else
	{
		/// ok recovering?
		_low_q_ct -= 3;
		if (_low_q_ct < 0) _low_q_ct = 0;
	}

	_movement = false;

	/// to check for movement (of the target).
	double dist = sqrt((double)((_px-_tx)*(_px-_tx)+(_py-_ty)*(_py-_ty)));

	if (fell || (dist > 2) || (sqrt(_dgx * _dgx + _dgy * _dgy) > 2.0))
	{
		_prev.PeerCopy(_mono);
		_px = _tx; _py = _ty;
	}

	/// target moved, all ok?
	if (dist > 5)
	{
		_movement = true;
		_last_movement = now;
	}

	///
	///
	/// computes the ray for the kin estimation.
#if defined(__QNXEurobot__)
	_gaze.computeRay (YARPEurobotHeadKin::KIN_LEFT, _prevRay, x, y); ///-ISIZE/2, y-ISIZE/2);
#else      // ----- #ifdef __QNXEurobot__  ----- 
	_gaze.computeRay (YARPBabybotHeadKin::KIN_LEFT, _prevRay, x, y); ///-ISIZE/2, y-ISIZE/2);
#endif     // ----- #ifdef __QNXEurobot__  ----- 
	_prev_gaze_x = x;
	_prev_gaze_y = y;

	///printf ("target: %d %d ray: %f %f %f\n", x, y, _prevRay(1), _prevRay(2), _prevRay(3));

	///
	///
	/// just a bit of display of results.
	YarpPixelBGR pix(255,0,0);

	YarpPixelBGR pixg(0,255,0);
	YarpPixelBGR pixb(0,0,255);
	YarpPixelBGR pixr(128,64,0);
	YarpPixelBGR pixk(0,0,0);
	YarpPixelBGR pixw(255,255,255);

	AddCircleOutline (dest, pixw, (int)x, (int)y, 5);
	AddCircleOutline (dest, pixk, (int)x, (int)y, 6);
	AddCircle (dest, pixk, (int)x, (int)y, 4);
	AddCrossHair (dest, pixw, (int)x+1, (int)y, 6);
	AddCrossHair (dest, pixw, (int)x-1, (int)y, 6);
	AddCrossHair (dest, pixw, (int)x, (int)y+1, 6);
	AddCrossHair (dest, pixw, (int)x, (int)y-1, 6);
	AddCrossHair (dest, pixr, (int)x, (int)y, 6);
	AddCircle (dest, pixw, (int)x, (int)y, 2);

	if (act_vector)
	{
		AddCircle (dest, pix, (int)(new_tx + _dgx), (int)(new_ty + _dgy), 3);
		AddCircle (dest, pix, (int)(new_tx2 + _dgx), (int)(new_ty2 + _dgy), 3);
		AddCircle (dest, pixb, (int)sub_x, (int)sub_y, 2);
		AddCircle (dest, pixb, (int)sub_x2, (int)sub_y2, 2);
	}

	_lock.Wait();
	_xx = x - ISIZE / 2;
	_yy = y - ISIZE / 2;
	_lock.Post();
}
  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;

#ifdef _DEBUG
	printf ("orientation: %lf\n", angle * radToDeg);
#endif
	int x1 = betterx + cos(angle) * 40;
	int y1 = bettery + sin(angle) * 40;
	int x2 = betterx - cos(angle) * 40;
	int y2 = bettery - sin(angle) * 40;
	AddSegment (m_backp[max_obj], red, x1, y1, x2, y2);
	AddRectangle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));

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

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

	return 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;
	AddSegment (m_backp[max_obj], red, x1, y1, x2, y2);
	AddRectangle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));

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

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

	// angle is the current orientation.
	// search for the best affordance.
	const double affordance = m_canonical_stats->GetPrincipalAffordance (max_obj);
	double o1 = angle+affordance;
	double o2 = angle-affordance;
	printf ("---- object: %d, affordance: %lf, test1: %lf, test2: %lf\n", max_obj, affordance*radToDeg, o1*radToDeg, o2*radToDeg);

	if (o1 > pi/2) o1 -= pi;
	else
	if (o1 < -pi/2) o1 += pi;
	
	if (o2 > pi/2) o2 -= pi;
	else
	if (o2 < -pi/2) o2 += pi;

	double score1 = 0;
	double score2 = 0;
	int bestaction1 = m_canonical_stats->GetBestActionGeneric (o1, score1);
	int bestaction2 = m_canonical_stats->GetBestActionGeneric (o2, score2);

	printf ("---- score(s) %lf %lf\n", score1, score2);

	if (score1 < score2)
		bestaction = bestaction1;
	else
		bestaction = bestaction2;

	printf ("the best action for this object in this position is %d\n", bestaction);

	return 0;
}