/**
* 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
			}
		}
	}
}
Example #2
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] );
		}
	}
}