/**
* Assign ids to blobs and fire blob events.
* This method tracks by proximity and best fit.
*/
void ofxCvBlobTracker::trackBlobs( const vector<ofxCvBlob>& _blobs ) {
unsigned int i, j, k;
// Push to history, clear
history.push_back( blobs );
if( history.size() > 4 ) {
history.erase( history.begin() );
}
blobs.clear();
// Load new blobs
for( i=0; i<_blobs.size(); i++ ) {
blobs.push_back( ofxCvTrackedBlob(_blobs[i]) );
}
vector<ofxCvTrackedBlob> *prev = &history[history.size()-1];
int cursize = blobs.size();
int prevsize = (*prev).size();
// now figure out the 'error' (distance) to all blobs in the previous
// frame. We are optimizing for the least change in distance.
// While this works really well we could also optimize for lowest
// deviation from predicted position, change in size etc...
for( i=0; i<cursize; i++ ) {
blobs[i].error.clear();
blobs[i].closest.clear();
for( j=0; j<prevsize; j++ ) {
//calc error - distance to blob in prev frame
float deviationX = blobs[i].centroid.x - (*prev)[j].centroid.x;
float deviationY = blobs[i].centroid.y - (*prev)[j].centroid.y;
float error = (float)sqrt( deviationX*deviationX + deviationY*deviationY );
blobs[i].error.push_back( error );
blobs[i].closest.push_back( j );
}
}
// sort so we can make a list of the closest blobs in the previous frame..
for( i=0; i<cursize; i++ ) {
// Bubble sort closest.
for( j=0; j<prevsize; j++ ) {
for( k=0; k<prevsize-1-j; k++ ) {
// ugly as hell, I know.
if( blobs[i].error[blobs[i].closest[k+1]]
< blobs[i].error[blobs[i].closest[k]] ) {
int tmp = blobs[i].closest[k]; // swap
blobs[i].closest[k] = blobs[i].closest[k+1];
blobs[i].closest[k+1] = tmp;
}
}
}
}
// Generate a matrix of all the possible choices.
// Then we will calculate the errors for every possible match
// and pick the matrix that has the lowest error.
// This is an NP complete approach and exponentially increases in complexity
// with the number of blobs. To remedy for each blob we will only
// consider the 4 closest blobs of the previous frame.
ids.clear();
// collect id's..
for( i=0; i<cursize; i++ ) {
ids.push_back( -1 );
}
extraIDs = cursize - prevsize;
if( extraIDs < 0 ) {
extraIDs = 0;
}
matrix.clear();
// FIXME: we could scale numcheck depending on how many blobs there are
// if we are tracking a lot of blobs, we could check less..
if( cursize <= 4 ) {
numcheck = 4;
} else if( cursize <= 6 ) {
numcheck = 3;
} else if( cursize <= 10 ) {
numcheck = 2;
} else {
numcheck = 1;
}
if( prevsize < numcheck ) {
numcheck = prevsize;
}
if( blobs.size() > 0 ) {
permute(0);
}
unsigned int num_results = matrix.size();
// loop through all the potential
// ID configurations and find one with lowest error
float best_error = 99999, error;
int best_error_ndx = -1;
for( j=0; j<num_results; j++ ) {
error = 0;
// get the error for each blob and sum
for( i=0; i<cursize; i++ ) {
//ofxCvTrackedBlob *f = 0;
if( matrix[j][i] != -1 ) {
error += blobs[i].error[matrix[j][i]];
}
}
if( error < best_error) {
best_error = error;
best_error_ndx = j;
}
}
// now that we know the optimal configuration,
// set the IDs and calculate some things..
if( best_error_ndx != -1 ) {
for( i=0; i<cursize; i++ ) {
if( matrix[best_error_ndx][i] != -1 ) {
blobs[i].id = (*prev)[matrix[best_error_ndx][i]].id;
} else {
blobs[i].id = -1;
}
if( blobs[i].id != -1 ) {
ofxCvTrackedBlob *oldblob = &(*prev)[matrix[best_error_ndx][i]];
blobs[i].deltaLoc.x = (blobs[i].centroid.x - oldblob->centroid.x);
blobs[i].deltaLoc.y = (blobs[i].centroid.y - oldblob->centroid.y);
blobs[i].deltaArea = blobs[i].area - oldblob->area;
blobs[i].predictedPos.x = blobs[i].centroid.x + blobs[i].deltaLoc.x;
blobs[i].predictedPos.y = blobs[i].centroid.y + blobs[i].deltaLoc.y;
blobs[i].deltaLocTotal.x = oldblob->deltaLocTotal.x + blobs[i].deltaLoc.x;
blobs[i].deltaLocTotal.y = oldblob->deltaLocTotal.y + blobs[i].deltaLoc.y;
} else {
blobs[i].deltaLoc = ofPoint( 0.0f, 0.0f );
blobs[i].deltaArea = 0;
blobs[i].predictedPos = blobs[i].centroid;
blobs[i].deltaLocTotal = ofPoint( 0.0f, 0.0f );
}
}
}
// fire events
//
// assign ID's for any blobs that are new this frame (ones that didn't get
// matched up with a blob from the previous frame).
for( i=0; i<cursize; i++ ) {
if(blobs[i].id == -1) {
blobs[i].id = currentID;
currentID ++;
if( currentID >= 65535 ) {
currentID = 0;
}
//doTouchEvent(blobs[i].getTouchData());
doBlobOn( blobs[i] );
} else {
float totalLength =
(float)sqrt( blobs[i].deltaLocTotal.x*blobs[i].deltaLocTotal.x
+ blobs[i].deltaLocTotal.y*blobs[i].deltaLocTotal.y );
if( totalLength >= minimumDisplacementThreshold ) {
//doUpdateEvent( blobs[i].getTouchData() );
doBlobMoved( blobs[i] );
blobs[i].deltaLocTotal = ofPoint( 0.0f, 0.0f );
}
}
}
// if a blob disappeared this frame, send a blob off event
// for each one in the last frame, see if it still exists in the new frame.
for( i=0; i<prevsize; i++ ) {
bool found = false;
for( j=0; j<cursize; j++ ) {
if( blobs[j].id == (*prev)[i].id ) {
found = true;
break;
}
}
if( !found ) {
if( ghost_frames == 0 ) {
//doUntouchEvent((*prev)[i].getTouchData());
doBlobOff( (*prev)[i] );
} else if( (*prev)[i].markedForDeletion ) {
(*prev)[i].framesLeft -= 1;
if( (*prev)[i].framesLeft <= 0 ) {
//doUntouchEvent( (*prev)[i].getTouchData() );
doBlobOff( (*prev)[i] );
} else {
blobs.push_back( (*prev)[i] ); // keep it around
// until framesleft = 0
}
} else {
(*prev)[i].markedForDeletion = true;
(*prev)[i].framesLeft = ghost_frames;
blobs.push_back( (*prev)[i] ); // keep it around
// until framesleft = 0
}
}
}
}
//assigns IDs to each blob in the contourFinder
void BlobTracker::track(ofxCvContourFinder* newBlobs)
{
//initialize ID's of all blobs
for(int i=0; i<newBlobs->nBlobs; i++)
newBlobs->blobs[i].id=-1;
//go through all tracked blobs to compute nearest new point
for(int i=0; i<trackedBlobs.size(); i++)
{
/******************************************************************
* *****************TRACKING FUNCTION TO BE USED*******************
* Replace 'trackKnn(...)' with any function that will take the
* current track and find the corresponding track in the newBlobs
* 'winner' should contain the index of the found blob or '-1' if
* there was no corresponding blob
*****************************************************************/
int winner = trackKnn(newBlobs, &(trackedBlobs[i]), 3, 0);
if(winner==-1) //track has died, mark it for deletion
{
//SEND BLOB OFF EVENT
doBlobOff( trackedBlobs[i] );
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
else //still alive, have to update
{
//if winning new blob was labeled winner by another track\
//then compare with this track to see which is closer
if(newBlobs->blobs[winner].id!=-1)
{
//find the currently assigned blob
int j; //j will be the index of it
for(j=0; j<trackedBlobs.size(); j++)
{
if(trackedBlobs[j].id==newBlobs->blobs[winner].id)
break;
}
if(j==trackedBlobs.size())//got to end without finding it
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
trackedBlobs[i] = newBlobs->blobs[winner];
}
else //found it, compare with current blob
{
double x = newBlobs->blobs[winner].centroid.x;
double y = newBlobs->blobs[winner].centroid.y;
double xOld = trackedBlobs[j].centroid.x;
double yOld = trackedBlobs[j].centroid.y;
double xNew = trackedBlobs[i].centroid.x;
double yNew = trackedBlobs[i].centroid.y;
double distOld = (x-xOld)*(x-xOld)+(y-yOld)*(y-yOld);
double distNew = (x-xNew)*(x-xNew)+(y-yNew)*(y-yNew);
//if this track is closer, update the ID of the blob
//otherwise delete this track.. it's dead
if(distNew<distOld) //update
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
//TODO--------------------------------------------------------------------------
//now the old winning blob has lost the win.
//I should also probably go through all the newBlobs
//at the end of this loop and if there are ones without
//any winning matches, check if they are close to this
//one. Right now I'm not doing that to prevent a
//recursive mess. It'll just be a new track.
//SEND BLOB OFF EVENT
doBlobOff( trackedBlobs[j] );
//mark the blob for deletion
trackedBlobs[j].id = -1;
//------------------------------------------------------------------------------
}
else //delete
{
//SEND BLOB OFF EVENT
doBlobOff( trackedBlobs[i] );
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
}
}
else //no conflicts, so simply update
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
}
}
}
//--Update All Current Tracks
//remove every track labeled as dead (ID='-1')
//find every track that's alive and copy it's data from newBlobs
for(int i=0; i<trackedBlobs.size(); i++)
{
if(trackedBlobs[i].id==-1) //dead
{
//erase track
trackedBlobs.erase(trackedBlobs.begin()+i,
trackedBlobs.begin()+i+1);
i--; //decrement one since we removed an element
}
else //living, so update it's data
{
for(int j=0; j<newBlobs->nBlobs; j++)
{
if(trackedBlobs[i].id==newBlobs->blobs[j].id)
{
//update track
trackedBlobs[i]=newBlobs->blobs[j];
//SEND BLOB MOVED EVENT
doBlobMoved( trackedBlobs[i] );
}
}
}
}
//--Add New Living Tracks
//now every new blob should be either labeled with a tracked ID or\
//have ID of -1... if the ID is -1... we need to make a new track
for(int i=0; i<newBlobs->nBlobs; i++)
{
if(newBlobs->blobs[i].id==-1)
{
//add new track
newBlobs->blobs[i].id=IDCounter++;
trackedBlobs.push_back(newBlobs->blobs[i]);
//SEND BLOB ON EVENT
doBlobOn( trackedBlobs[i] );
}
}
}
