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