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 TWindow::MessageReceived(BMessage* m) { bool active = fFatBits->Active(); switch (m->what) { case msg_show_info: if (active) { fInfoBarState = !fInfoBarState; ShowInfo(!fShowInfo); } break; case msg_toggle_grid: if (active) SetGrid(!fShowGrid); break; case msg_grow: if (active) ResizeWindow(true); break; case msg_shrink: if (active) ResizeWindow(false); break; case msg_make_square: if (active) { if (fHPixelCount == fVPixelCount) break; int32 big = (fHPixelCount > fVPixelCount) ? fHPixelCount : fVPixelCount; ResizeWindow(big, big); } break; case msg_shrink_pixel: if (active) SetPixelSize(false); break; case msg_grow_pixel: if (active) SetPixelSize(true); break; case msg_add_cross_hair: if (active && fShowInfo) AddCrossHair(); break; case msg_remove_cross_hair: if (active && fShowInfo) RemoveCrossHair(); break; case msg_freeze: if (active) SetFlags(B_OUTLINE_RESIZE | B_NOT_ZOOMABLE | B_NOT_RESIZABLE); else SetFlags(B_OUTLINE_RESIZE | B_NOT_ZOOMABLE); fFatBits->MakeActive(!fFatBits->Active()); break; case msg_stick: fFatBits->MakeSticked(!fFatBits->Sticked()); break; case msg_save: { // freeze the image here, unfreeze after dump or cancel fFatBits->StartSave(); BMessenger messenger(this); BMessage message(msg_dump); fSavePanel = new BFilePanel(B_SAVE_PANEL, &messenger, 0, 0, false, &message); fSavePanel->SetSaveText("Bitmaps.h"); fSavePanel->Show(); } break; case msg_dump: { delete fSavePanel; entry_ref dirRef; char* name; m->FindRef("directory", &dirRef); m->FindString((const char*)"name",(const char**) &name); fFatBits->SaveImage(&dirRef, name); } break; case B_CANCEL: // image is frozen before the FilePanel is shown fFatBits->EndSave(); break; case msg_copy_image: fFatBits->CopyImage(); break; default: BWindow::MessageReceived(m); break; } }