//assigns IDs to each blob in the contourFinder
void ofxTracking::track(ofxContourFinder* newBlobs){
numEnter = 0;
numLeave = 0;
//printf("%d\n",trackedBlobs.size());
/****************************************************************************
//Blob tracking
****************************************************************************/
//initialize ID's of all blobs
for(int i=0; i<newBlobs->nBlobs; i++)
newBlobs->blobs[i].id=-1;
size = trackedBlobs.size();
// STEP 1: Blob matching
//
//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, false);
if(winner == -1) { //track has died, mark it 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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
//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.
//erase calibrated blob from map
//calibratedBlobs.erase(trackedBlobs[j].id);
//mark the blob for deletion
trackedBlobs[j].id = -1;
//-----------------------------------------------------
} else { //delete
//erase calibrated blob from map
//calibratedBlobs.erase(trackedBlobs[i].id);
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
}
} else {//no conflicts, so simply update
newBlobs->blobs[winner].id = trackedBlobs[i].id;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
}
}
}
// AlexP
// save the current time since we will be using it a lot
int now = ofGetElapsedTimeMillis();
// STEP 2: Blob update
//
//--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
numLeave++;
//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
ofPoint tempLastCentroid = trackedBlobs[i].centroid; // assign the new centroid to the old
trackedBlobs[i] = newBlobs->blobs[j];
trackedBlobs[i].lastCentroid = tempLastCentroid;
ofPoint tD;
//get the Differences in position
tD.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//calculate the acceleration
float posDelta = sqrtf((tD.x*tD.x)+(tD.y*tD.y));
// AlexP
// now, filter the blob position based on MOVEMENT_FILTERING value
// the MOVEMENT_FILTERING ranges [0,15] so we will have that many filtering steps
// Here we have a weighted low-pass filter
// adaptively adjust the blob position filtering strength based on blob movement
// http://www.wolframalpha.com/input/?i=plot+1/exp(x/15)+and+1/exp(x/10)+and+1/exp(x/5)+from+0+to+100
float a = 1.0f - 1.0f / expf(posDelta / (1.0f + (float)MOVEMENT_FILTERING*10));
trackedBlobs[i].centroid.x = a * trackedBlobs[i].centroid.x + (1-a) * trackedBlobs[i].lastCentroid.x;
trackedBlobs[i].centroid.y = a * trackedBlobs[i].centroid.y + (1-a) * trackedBlobs[i].lastCentroid.y;
//get the Differences in position
trackedBlobs[i].D.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//if( abs((int)trackedBlobs[i].D.x) > 1 || abs((int)trackedBlobs[i].D.y) > 1) {
// printf("\nUNUSUAL BLOB @ %f\n-----------------------\ntrackedBlobs[%i]\nD = (%f, %f)\nXY= (%f, %f)\nlastTimeTimeWasChecked = %f\nsitting = %f\n",
// ofGetElapsedTimeMillis(),
// i,
// trackedBlobs[i].D.x, trackedBlobs[i].D.y,
// trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y,
// trackedBlobs[i].lastTimeTimeWasChecked,
// trackedBlobs[i].downTime,
// trackedBlobs[i].sitting
// );
// }
//calculate the acceleration again
tD = trackedBlobs[i].D;
trackedBlobs[i].maccel = sqrtf((tD.x* tD.x)+(tD.y*tD.y)) / (now - trackedBlobs[i].lastTimeTimeWasChecked);
//calculate the age
trackedBlobs[i].age = ofGetElapsedTimef() - trackedBlobs[i].downTime;
//set sitting (held length)
if(trackedBlobs[i].maccel < 7) { //1 more frame of sitting
if(trackedBlobs[i].sitting != -1)
trackedBlobs[i].sitting = ofGetElapsedTimef() - trackedBlobs[i].downTime;
} else
trackedBlobs[i].sitting = -1;
//printf("time: %f\n", ofGetElapsedTimeMillis());
//printf("%i age: %f, downTimed at: %f\n", i, trackedBlobs[i].age, trackedBlobs[i].downTime);
//if blob has been 'holding/sitting' for 1 second send a held event
if(trackedBlobs[i].sitting > 1.0f){
//held event only happens once so set to -1
trackedBlobs[i].sitting = -1;
}
// AlexP
// The last lastTimeTimeWasChecked is updated at the end after all acceleration values are calculated
trackedBlobs[i].lastTimeTimeWasChecked = now;
}
}
}
}
// STEP 3: add tracked blobs to TouchEvents
//--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++;
IDCounter++;
newBlobs->blobs[i].downTime = ofGetElapsedTimef();
//newBlobs->blobs[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
//random color for blob. Could be useful?
int r = ofRandom(0, 255);
int g = ofRandom(0, 255);
int b = ofRandom(0, 255);
//Convert to hex
int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
//Set color
newBlobs->blobs[i].color = rgbNum;
trackedBlobs.push_back(newBlobs->blobs[i]);
numEnter++;
if (numEnter > 20){
printf("something wrong!\n");
}
}
}
/****************************************************************************
//Finger tracking
****************************************************************************/
//initialize ID's of all blobs
for(int i=0; i<newBlobs->nFingers; i++)
newBlobs->fingers[i].id=-1;
// STEP 1: Finger matching
//
//go through all tracked blobs to compute nearest new point
for(int i = 0; i < trackedFingers.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, &(trackedFingers[i]), 3, 0, true);
if(winner == -1) { //track has died, mark it for deletion
trackedFingers[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->fingers[winner].id!=-1) {
//find the currently assigned finger
int j; //j will be the index of it
for(j=0; j<trackedFingers.size(); j++){
if(trackedFingers[j].id==newBlobs->fingers[winner].id)
break;
}
if(j==trackedFingers.size()){ //got to end without finding it
newBlobs->fingers[winner].id = trackedFingers[i].id;
newBlobs->fingers[winner].age = trackedFingers[i].age;
newBlobs->fingers[winner].sitting = trackedFingers[i].sitting;
newBlobs->fingers[winner].downTime = trackedFingers[i].downTime;
newBlobs->fingers[winner].color = trackedFingers[i].color;
newBlobs->fingers[winner].lastTimeTimeWasChecked = trackedFingers[i].lastTimeTimeWasChecked;
trackedFingers[i] = newBlobs->fingers[winner];
} else {//found it, compare with current blob
double x = newBlobs->fingers[winner].centroid.x;
double y = newBlobs->fingers[winner].centroid.y;
double xOld = trackedFingers[j].centroid.x;
double yOld = trackedFingers[j].centroid.y;
double xNew = trackedFingers[i].centroid.x;
double yNew = trackedFingers[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->fingers[winner].id = trackedFingers[i].id;
newBlobs->fingers[winner].age = trackedFingers[i].age;
newBlobs->fingers[winner].sitting = trackedFingers[i].sitting;
newBlobs->fingers[winner].downTime = trackedFingers[i].downTime;
newBlobs->fingers[winner].color = trackedFingers[i].color;
newBlobs->fingers[winner].lastTimeTimeWasChecked = trackedFingers[i].lastTimeTimeWasChecked;
//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.
//erase calibrated blob from map
//calibratedBlobs.erase(trackedBlobs[j].id);
//mark the blob for deletion
trackedFingers[j].id = -1;
//------------------------------------------------------------------------------
} else { //delete
//erase calibrated blob from map
//calibratedBlobs.erase(trackedBlobs[i].id);
//mark the blob for deletion
trackedFingers[i].id = -1;
}
}
} else { //no conflicts, so simply update
newBlobs->fingers[winner].id = trackedFingers[i].id;
newBlobs->fingers[winner].age = trackedFingers[i].age;
newBlobs->fingers[winner].sitting = trackedFingers[i].sitting;
newBlobs->fingers[winner].downTime = trackedFingers[i].downTime;
newBlobs->fingers[winner].color = trackedFingers[i].color;
newBlobs->fingers[winner].lastTimeTimeWasChecked = trackedFingers[i].lastTimeTimeWasChecked;
}
}
}
// AlexP
// save the current time since we will be using it a lot
now = ofGetElapsedTimeMillis();
// STEP 2: Blob update
//
//--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 < trackedFingers.size(); i++){
if(trackedFingers[i].id == -1) { //dead
trackedFingers.erase(trackedFingers.begin()+i, trackedFingers.begin()+i+1); //erase track
i--; //decrement one since we removed an element
} else { //living, so update it's data
for(int j = 0; j < newBlobs->nFingers; j++){
if(trackedFingers[i].id == newBlobs->fingers[j].id) {
//update track
ofPoint tempLastCentroid = trackedFingers[i].centroid; // assign the new centroid to the old
trackedFingers[i] = newBlobs->fingers[j];
trackedFingers[i].lastCentroid = tempLastCentroid;
ofPoint tD;
//get the Differences in position
tD.set(trackedFingers[i].centroid.x - trackedFingers[i].lastCentroid.x,
trackedFingers[i].centroid.y - trackedFingers[i].lastCentroid.y);
//calculate the acceleration
float posDelta = sqrtf((tD.x*tD.x)+(tD.y*tD.y));
// AlexP
// now, filter the blob position based on MOVEMENT_FILTERING value
// the MOVEMENT_FILTERING ranges [0,15] so we will have that many filtering steps
// Here we have a weighted low-pass filter
// adaptively adjust the blob position filtering strength based on blob movement
// http://www.wolframalpha.com/input/?i=plot+1/exp(x/15)+and+1/exp(x/10)+and+1/exp(x/5)+from+0+to+100
float a = 1.0f - 1.0f / expf(posDelta / (1.0f + (float)MOVEMENT_FILTERING*10));
trackedFingers[i].centroid.x = a * trackedFingers[i].centroid.x + (1-a) * trackedFingers[i].lastCentroid.x;
trackedFingers[i].centroid.y = a * trackedFingers[i].centroid.y + (1-a) * trackedFingers[i].lastCentroid.y;
//get the Differences in position
trackedFingers[i].D.set( trackedFingers[i].centroid.x - trackedFingers[i].lastCentroid.x,
trackedFingers[i].centroid.y - trackedFingers[i].lastCentroid.y);
//if( abs((int)trackedBlobs[i].D.x) > 1 || abs((int)trackedBlobs[i].D.y) > 1) {
// printf("\nUNUSUAL BLOB @ %f\n-----------------------\ntrackedBlobs[%i]\nD = (%f, %f)\nXY= (%f, %f)\nlastTimeTimeWasChecked = %f\nsitting = %f\n",
// ofGetElapsedTimeMillis(),
// i,
// trackedBlobs[i].D.x, trackedBlobs[i].D.y,
// trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y,
// trackedBlobs[i].lastTimeTimeWasChecked,
// trackedBlobs[i].downTime,
// trackedBlobs[i].sitting
// );
// }
//calculate the acceleration again
tD = trackedFingers[i].D;
trackedFingers[i].maccel = sqrtf((tD.x* tD.x)+(tD.y*tD.y)) / (now - trackedFingers[i].lastTimeTimeWasChecked);
//calculate the age
trackedFingers[i].age = ofGetElapsedTimef() - trackedFingers[i].downTime;
//set sitting (held length)
if(trackedFingers[i].maccel < 7) { //1 more frame of sitting
if(trackedFingers[i].sitting != -1)
trackedFingers[i].sitting = ofGetElapsedTimef() - trackedFingers[i].downTime;
} else
trackedFingers[i].sitting = -1;
//printf("time: %f\n", ofGetElapsedTimeMillis());
//printf("%i age: %f, downTimed at: %f\n", i, trackedBlobs[i].age, trackedBlobs[i].downTime);
//if blob has been 'holding/sitting' for 1 second send a held event
if(trackedFingers[i].sitting > 1.0f){
//held event only happens once so set to -1
trackedFingers[i].sitting = -1;
//TouchEvents.notifyTouchHeld(NULL);
}
// AlexP
// The last lastTimeTimeWasChecked is updated at the end after all acceleration values are calculated
trackedFingers[i].lastTimeTimeWasChecked = now;
}
}
}
}
// STEP 3: add tracked blobs to TouchEvents
//--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->nFingers; i++) {
if(newBlobs->fingers[i].id==-1){
//add new track
newBlobs->fingers[i].id = IDCounter++;
newBlobs->fingers[i].downTime = ofGetElapsedTimef();
//newBlobs->fingers[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
//random color for blob. Could be useful?
int r = ofRandom(0, 255);
int g = ofRandom(0, 255);
int b = ofRandom(0, 255);
//Convert to hex
int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
//Set color
newBlobs->fingers[i].color = rgbNum;
trackedFingers.push_back(newBlobs->fingers[i]);
}
}
}
//assigns IDs to each blob in the contourFinder
void BlobTracker::track(ContourFinder* 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++)
{
//cout<<"BlobTracker.trackedBlobs:"<< &(trackedBlobs[i])<<endl;
/******************************************************************
* *****************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
messenger = trackedBlobs[i];
//erase calibrated blob from map
calibratedBlobs.erase(messenger.id);
//TouchEvents.notifyTouchUp(NULL);
//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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
//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
messenger = trackedBlobs[j];
//erase calibrated blob from map
calibratedBlobs.erase(messenger.id);
//mark the blob for deletion
trackedBlobs[j].id = -1;
//------------------------------------------------------------------------------
}
else //delete
{
//SEND BLOB OFF EVENT
messenger = trackedBlobs[i];
//erase calibrated blob from map
calibratedBlobs.erase(messenger.id);
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
}
}
else //no conflicts, so simply update
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
}
}
}
//--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
ofPoint tempLastCentroid = trackedBlobs[i].centroid; // assign the new centroid to the old
trackedBlobs[i]=newBlobs->blobs[j];
trackedBlobs[i].lastCentroid = tempLastCentroid;
//SEND BLOB MOVED EVENT
//messenger = trackedBlobs[i];
//get the Differences in position
trackedBlobs[i].D.set((trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x) / (ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked),
(trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y) / (ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked));
cout <<"trackedBlobs[i].D:"<<trackedBlobs[i].D.x<<":"<<trackedBlobs[i].D.y << endl;
//calibrated accelleration
ofPoint tD2 = trackedBlobs[i].D;
trackedBlobs[i].maccel = sqrtf((tD2.x* tD2.x)+(tD2.y*tD2.y)) /(ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked)/1000.0f;
cout <<"trackedBlobs[i].D:"<<trackedBlobs[i].maccel<< endl;
trackedBlobs[i].lastTimeTimeWasChecked = ofGetElapsedTimef();
//add to calibration map
calibratedBlobs[trackedBlobs[i].id] = 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++;
newBlobs->blobs[i].downTime = ofGetElapsedTimef();
newBlobs->blobs[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
//random color for blob. Could be useful?
int r = ofRandom(0, 255);
int g = ofRandom(0, 255);
int b = ofRandom(0, 255);
//Convert to hex
int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
//Set color
newBlobs->blobs[i].color = rgbNum;
//Add to blob messenger
messenger = newBlobs->blobs[i];
//add to calibrated blob map
calibratedBlobs[messenger.id] = messenger;
//Send Event
//TouchEvents.notifyTouchDown(NULL);
trackedBlobs.push_back(newBlobs->blobs[i]);
}
}
}
//assigns IDs to each blob in the contourFinder
void BlobTracker::track(ContourFinder* 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
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating){
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchUp(NULL);
}
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
//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
TouchEvents.messenger = trackedBlobs[j];
if(isCalibrating){
TouchEvents.RAWmessenger = trackedBlobs[j];
TouchEvents.notifyRAWTouchUp(NULL);
}
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//mark the blob for deletion
trackedBlobs[j].id = -1;
//------------------------------------------------------------------------------
}
else //delete
{
//SEND BLOB OFF EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating){
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchUp(NULL);
}
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
}
}
else //no conflicts, so simply update
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
}
}
}
//--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
ofPoint tempLastCentroid = trackedBlobs[i].centroid; // assign the new centroid to the old
trackedBlobs[i]=newBlobs->blobs[j];
trackedBlobs[i].lastCentroid = tempLastCentroid;
//get the Differences in position
trackedBlobs[i].D.set((trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x) / (ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked),
(trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y) / (ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked));
//printf("D(%f, %f)\n", trackedBlobs[i].D.x, trackedBlobs[i].D.y);
//if( abs((int)trackedBlobs[i].D.x) > 1 || abs((int)trackedBlobs[i].D.y) > 1) {
// printf("\nUNUSUAL BLOB @ %f\n-----------------------\ntrackedBlobs[%i]\nD = (%f, %f)\nXY= (%f, %f)\nlastTimeTimeWasChecked = %f\nsitting = %f\n",
// ofGetElapsedTimef(),
// i,
// trackedBlobs[i].D.x, trackedBlobs[i].D.y,
// trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y,
// trackedBlobs[i].lastTimeTimeWasChecked,
// trackedBlobs[i].downTime,
// trackedBlobs[i].sitting
// );
// }
//calculate the accelleration
ofPoint tD = trackedBlobs[i].D;
trackedBlobs[i].maccel = sqrtf((tD.x* tD.x)+(tD.y*tD.y)/(ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked));
trackedBlobs[i].lastTimeTimeWasChecked = ofGetElapsedTimef();
//calculate the age
trackedBlobs[i].age = ofGetElapsedTimef() - trackedBlobs[i].downTime;
//if not moving more than min_movement_threshold then set to same position as last frame
if(trackedBlobs[i].maccel < MIN_MOVEMENT_THRESHOLD)
{ //this helps avoid jittery blobs
trackedBlobs[i].centroid.x = trackedBlobs[i].lastCentroid.x;
trackedBlobs[i].centroid.y = trackedBlobs[i].lastCentroid.y;
}
//set sitting (held length)
if(trackedBlobs[i].maccel < 7)
{ //1 more frame of sitting
if(trackedBlobs[i].sitting != -1)
trackedBlobs[i].sitting = ofGetElapsedTimef() - trackedBlobs[i].downTime;
}
else {
trackedBlobs[i].sitting = -1;
}
//printf("time: %f\n", ofGetElapsedTimef());
//printf("%i age: %f, downTimed at: %f\n", i, trackedBlobs[i].age, trackedBlobs[i].downTime);
//if blob has been 'holding/sitting' for 1 second send a held event
if(trackedBlobs[i].sitting > 1.0f){
//SEND BLOB HELD EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating){
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchHeld(NULL);
}
//calibrated values
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
calibrate->cameraToScreenPosition(TouchEvents.messenger.lastCentroid.x, TouchEvents.messenger.lastCentroid.y);
//Calibrated dx/dy
TouchEvents.messenger.D.set((TouchEvents.messenger.centroid.x - TouchEvents.messenger.lastCentroid.x) / (ofGetElapsedTimef() - TouchEvents.messenger.lastTimeTimeWasChecked),
(TouchEvents.messenger.centroid.y - TouchEvents.messenger.lastCentroid.y) / (ofGetElapsedTimef() - TouchEvents.messenger.lastTimeTimeWasChecked));
TouchEvents.messenger.lastTimeTimeWasChecked = ofGetElapsedTimef();
//calibrated accelleration
ofPoint tD2 = TouchEvents.messenger.D;
TouchEvents.messenger.maccel = sqrtf((tD2.x* tD2.x)+(tD2.y*tD2.y)/(ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked));
//add to calibration map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
//held event only happens once so set to -1
trackedBlobs[i].sitting = -1;
TouchEvents.notifyTouchHeld(NULL);
} else {
//printf("(%f, %f) -> (%f, %f) \n", trackedBlobs[i].lastCentroid.x, trackedBlobs[i].lastCentroid.y, trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y);
//SEND BLOB MOVED EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating){
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchMoved(NULL);
}
//calibrated values
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
calibrate->cameraToScreenPosition(TouchEvents.messenger.lastCentroid.x, TouchEvents.messenger.lastCentroid.y);
//Calibrated dx/dy
TouchEvents.messenger.D.set((TouchEvents.messenger.centroid.x - TouchEvents.messenger.lastCentroid.x) / (ofGetElapsedTimef() - TouchEvents.messenger.lastTimeTimeWasChecked),
(TouchEvents.messenger.centroid.y - TouchEvents.messenger.lastCentroid.y) / (ofGetElapsedTimef() - TouchEvents.messenger.lastTimeTimeWasChecked));
TouchEvents.messenger.lastTimeTimeWasChecked = ofGetElapsedTimef();
//printf("d(%0.4f, %0.4f)\n", TouchEvents.messenger.D.x, TouchEvents.messenger.D.y);
//calibrated accelleration
ofPoint tD2 = TouchEvents.messenger.D;
TouchEvents.messenger.maccel = sqrtf((tD2.x* tD2.x)+(tD2.y*tD2.y)/(ofGetElapsedTimef() - trackedBlobs[i].lastTimeTimeWasChecked));
//add to calibration map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
TouchEvents.notifyTouchMoved(NULL);
}
}
}
}
}
//--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++;
newBlobs->blobs[i].downTime = ofGetElapsedTimef();
//newBlobs->blobs[i].lastTimeTimeWasChecked = ofGetElapsedTimef();
//random color for blob. Could be useful?
int r = ofRandom(0, 255);
int g = ofRandom(0, 255);
int b = ofRandom(0, 255);
//Convert to hex
int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
//Set color
newBlobs->blobs[i].color = rgbNum;
//Add to blob messenger
TouchEvents.messenger = newBlobs->blobs[i];
if(isCalibrating){
TouchEvents.RAWmessenger = newBlobs->blobs[i];
TouchEvents.notifyRAWTouchDown(NULL);
}
calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
//add to calibrated blob map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
//Send Event
TouchEvents.notifyTouchDown(NULL);
trackedBlobs.push_back(newBlobs->blobs[i]);
}
}
}
//assigns IDs to each blob in the contourFinder
void BlobTracker::track(ContourFinder* newBlobs)
{
/*********************************************************************
//Object tracking
*********************************************************************/
for(std::map<int,Blob>::iterator tracked=trackedObjects.begin();tracked != trackedObjects.end(); ++tracked)
{//iterate through the tracked blobs and check if any of these are present in current blob list, else delete it (mark id = -1)
int id= tracked->second.id;
bool isFound=false; // The variable to check if the blob is in the new blobs list or not
for(std::vector<Blob>::iterator blob = newBlobs->objects.begin();blob!=newBlobs->objects.end();++blob)
{
if(blob->id == id)
{
isFound=true;
break;
}
}
if(!isFound) // current tracked blob was not found in the new blob list
{
trackedObjects[id].id = -1;
}
}
//handle the object tracking if present
for (int i = 0; i < newBlobs->nObjects; i++)
{
int ID = newBlobs->objects[i].id;
int now = ofGetElapsedTimeMillis();
if(trackedObjects[ID].id == -1) //If this blob has appeared in the current frame
{
calibratedObjects[i]=newBlobs->objects[i];
calibratedObjects[i].D.x=0;
calibratedObjects[i].D.y=0;
calibratedObjects[i].maccel=0;
calibratedObjects[i].A = 0;
calibratedObjects[i].raccel = 0;
//Camera to Screen Position Conversion
calibratedObjects[i].centroid.x/=camWidth;
calibratedObjects[i].centroid.y/=camHeight;
calibratedObjects[i].angleBoundingRect.width/=camWidth;
calibratedObjects[i].angleBoundingRect.height/=camHeight;
}
else //Do all the calculations
{
float xOld = trackedObjects[ID].centroid.x;
float yOld = trackedObjects[ID].centroid.y;
float xNew = newBlobs->objects[i].centroid.x;
float yNew = newBlobs->objects[i].centroid.y;
//converting xNew and yNew to calibrated ones
xNew/=camWidth;
yNew/=camHeight;
double dx = xNew-xOld;
double dy = yNew-yOld;
calibratedObjects[i] = newBlobs->objects[i];
calibratedObjects[i].D.x = dx;
calibratedObjects[i].D.y = dy;
calibratedObjects[i].maccel = sqrtf((dx*dx+dy*dy)/(now - trackedObjects[ID].lastTimeTimeWasChecked));
calibratedObjects[i].A = (((calibratedObjects[i].angle - trackedObjects[ID].angle)/(2*PI))/(now - trackedObjects[ID].lastTimeTimeWasChecked));
calibratedObjects[i].raccel = (calibratedObjects[i].A - trackedObjects[ID].A)/(now - trackedObjects[ID].lastTimeTimeWasChecked);
calibratedObjects[i].centroid.x/=camWidth;
calibratedObjects[i].centroid.y/=camHeight;
calibratedObjects[i].angleBoundingRect.width/=camWidth;
calibratedObjects[i].angleBoundingRect.height/=camHeight;
}
trackedObjects[ID] = newBlobs->objects[i];
trackedObjects[ID].lastTimeTimeWasChecked = now;
trackedObjects[ID].centroid.x = calibratedObjects[i].centroid.x;
trackedObjects[ID].centroid.y = calibratedObjects[i].centroid.y;
}
/****************************************************************************
//Finger tracking
****************************************************************************/
//initialize ID's of all blobs
for(int i=0; i<newBlobs->nBlobs; i++)
newBlobs->blobs[i].id=-1;
// STEP 1: Blob matching
//
//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
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating)
{
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchUp(NULL);
}
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
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;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
//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
TouchEvents.messenger = trackedBlobs[j];
if(isCalibrating)
{
TouchEvents.RAWmessenger = trackedBlobs[j];
TouchEvents.notifyRAWTouchUp(NULL);
}
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//mark the blob for deletion
trackedBlobs[j].id = -1;
//------------------------------------------------------------------------------
}
else //delete
{
//SEND BLOB OFF EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating)
{
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchUp(NULL);
}
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
//erase calibrated blob from map
calibratedBlobs.erase(TouchEvents.messenger.id);
TouchEvents.notifyTouchUp(NULL);
//mark the blob for deletion
trackedBlobs[i].id = -1;
}
}
}
else //no conflicts, so simply update
{
newBlobs->blobs[winner].id = trackedBlobs[i].id;
newBlobs->blobs[winner].age = trackedBlobs[i].age;
newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
newBlobs->blobs[winner].color = trackedBlobs[i].color;
newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
}
}
}
// AlexP
// save the current time since we will be using it a lot
int now = ofGetElapsedTimeMillis();
// STEP 2: Blob update
//
//--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
ofPoint tempLastCentroid = trackedBlobs[i].centroid; // assign the new centroid to the old
trackedBlobs[i] = newBlobs->blobs[j];
trackedBlobs[i].lastCentroid = tempLastCentroid;
ofPoint tD;
//get the Differences in position
tD.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//calculate the acceleration
float posDelta = sqrtf((tD.x*tD.x)+(tD.y*tD.y));
// AlexP
// now, filter the blob position based on MOVEMENT_FILTERING value
// the MOVEMENT_FILTERING ranges [0,15] so we will have that many filtering steps
// Here we have a weighted low-pass filter
// adaptively adjust the blob position filtering strength based on blob movement
// http://www.wolframalpha.com/input/?i=plot+1/exp(x/15)+and+1/exp(x/10)+and+1/exp(x/5)+from+0+to+100
float a = 1.0f - 1.0f / expf(posDelta / (1.0f + (float)MOVEMENT_FILTERING*10));
trackedBlobs[i].centroid.x = a * trackedBlobs[i].centroid.x + (1-a) * trackedBlobs[i].lastCentroid.x;
trackedBlobs[i].centroid.y = a * trackedBlobs[i].centroid.y + (1-a) * trackedBlobs[i].lastCentroid.y;
//get the Differences in position
trackedBlobs[i].D.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//if( abs((int)trackedBlobs[i].D.x) > 1 || abs((int)trackedBlobs[i].D.y) > 1) {
// printf("\nUNUSUAL BLOB @ %f\n-----------------------\ntrackedBlobs[%i]\nD = (%f, %f)\nXY= (%f, %f)\nlastTimeTimeWasChecked = %f\nsitting = %f\n",
// ofGetElapsedTimeMillis(),
// i,
// trackedBlobs[i].D.x, trackedBlobs[i].D.y,
// trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y,
// trackedBlobs[i].lastTimeTimeWasChecked,
// trackedBlobs[i].downTime,
// trackedBlobs[i].sitting
// );
// }
//calculate the acceleration again
tD = trackedBlobs[i].D;
trackedBlobs[i].maccel = sqrtf((tD.x* tD.x)+(tD.y*tD.y)) / (now - trackedBlobs[i].lastTimeTimeWasChecked);
//calculate the age
trackedBlobs[i].age = ofGetElapsedTimef() - trackedBlobs[i].downTime;
//set sitting (held length)
if(trackedBlobs[i].maccel < 7)
{ //1 more frame of sitting
if(trackedBlobs[i].sitting != -1)
trackedBlobs[i].sitting = ofGetElapsedTimef() - trackedBlobs[i].downTime;
}
else
trackedBlobs[i].sitting = -1;
//printf("time: %f\n", ofGetElapsedTimeMillis());
//printf("%i age: %f, downTimed at: %f\n", i, trackedBlobs[i].age, trackedBlobs[i].downTime);
//if blob has been 'holding/sitting' for 1 second send a held event
if(trackedBlobs[i].sitting > 1.0f)
{
//SEND BLOB HELD EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating)
{
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchHeld(NULL);
}
//calibrated values
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
TouchEvents.messenger.lastCentroid.x/=camWidth;
TouchEvents.messenger.lastCentroid.y/=camHeight;
//Calibrated dx/dy
TouchEvents.messenger.D.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//calibrated acceleration
ofPoint tD = TouchEvents.messenger.D;
TouchEvents.messenger.maccel = sqrtf((tD.x*tD.x)+(tD.y*tD.y)) / (now - TouchEvents.messenger.lastTimeTimeWasChecked);
TouchEvents.messenger.lastTimeTimeWasChecked = now;
//add to calibration map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
//held event only happens once so set to -1
trackedBlobs[i].sitting = -1;
TouchEvents.notifyTouchHeld(NULL);
}
else
{
//printf("(%f, %f) -> (%f, %f) \n", trackedBlobs[i].lastCentroid.x, trackedBlobs[i].lastCentroid.y, trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y);
//SEND BLOB MOVED EVENT
TouchEvents.messenger = trackedBlobs[i];
if(isCalibrating)
{
TouchEvents.RAWmessenger = trackedBlobs[i];
TouchEvents.notifyRAWTouchMoved(NULL);
}
//calibrated values
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
TouchEvents.messenger.lastCentroid.x/=camWidth;
TouchEvents.messenger.lastCentroid.y/=camHeight;
//Calibrated dx/dy
TouchEvents.messenger.D.set(trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x,
trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y);
//printf("d(%0.4f, %0.4f)\n", TouchEvents.messenger.D.x, TouchEvents.messenger.D.y);
//calibrated acceleration
ofPoint tD = TouchEvents.messenger.D;
TouchEvents.messenger.maccel = sqrtf((tD.x*tD.x)+(tD.y*tD.y)) / (now - TouchEvents.messenger.lastTimeTimeWasChecked);
TouchEvents.messenger.lastTimeTimeWasChecked = now;
//add to calibration map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
TouchEvents.notifyTouchMoved(NULL);
}
// AlexP
// The last lastTimeTimeWasChecked is updated at the end after all acceleration values are calculated
trackedBlobs[i].lastTimeTimeWasChecked = now;
}
}
}
}
// STEP 3: add tracked blobs to TouchEvents
//--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++;
newBlobs->blobs[i].downTime = ofGetElapsedTimef();
//newBlobs->blobs[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
//random color for blob. Could be useful?
int r = ofRandom(0, 255);
int g = ofRandom(0, 255);
int b = ofRandom(0, 255);
//Convert to hex
int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
//Set color
newBlobs->blobs[i].color = rgbNum;
//Add to blob messenger
TouchEvents.messenger = newBlobs->blobs[i];
if(isCalibrating)
{
TouchEvents.RAWmessenger = newBlobs->blobs[i];
TouchEvents.notifyRAWTouchDown(NULL);
}
TouchEvents.messenger.boundingRect.width/=camWidth;
TouchEvents.messenger.boundingRect.height/=camHeight;
TouchEvents.messenger.centroid.x/=camWidth;
TouchEvents.messenger.centroid.y/=camHeight;
//add to calibrated blob map
calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
//Send Event
TouchEvents.notifyTouchDown(NULL);
trackedBlobs.push_back(newBlobs->blobs[i]);
}
}
}
//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] );
}
}
}
//--------------------------------------------------------------------------------
void blobTracker::track(vector<ofVec3f>& points, int aliveFrames, float speedThreshold){
///Массив с информацией о пользователях на новом кадре
vector<touch> newTouchList;
///Шаг 1. Задаем всем новым блобам - вероятным пользователям id = -1 и копируем данные блоба
for(unsigned int i = 0; i < points.size(); i++){
touch t;
t.id = -1;
t.blob = ofxCvBlob();
t.blob.centroid = points[i];
t.framesToLive = aliveFrames;
newTouchList.push_back(t);
newTouchList.back().filter.setup(2,15,1,1);
newTouchList.back().filter.setState(points[i], 0.1);
}
///Шаг 2. Делаем трекинг блобов с предыдущего кадра - находим среди новых ближайший
for(unsigned int i = 0; i < touchList.size(); i++){
int resultIndex = trackKnn(&newTouchList, &(touchList[i].blob), 3);
if(resultIndex == -1){
///Защита от промаргивания
touchList[i].framesToLive--;
if(touchList[i].framesToLive <= 0)
touchList[i].id = -1;///Удаление пользователя
}
else{///Пользователь найден в списке
///Если для нового блоба оказался другой возможный вариант - сравниваем их
if(newTouchList[resultIndex].id != -1){
unsigned int j;
for(j = 0; j < touchList.size(); j++){
if(touchList[j].id == newTouchList[resultIndex].id)
break;
}
if(j == touchList.size()){
newTouchList[resultIndex].id = touchList[i].id;
touchList[i] = newTouchList[resultIndex];
}
///Сравнение с блобом-"конкурентом"
else{
double x = newTouchList[resultIndex].blob.centroid.x;
double y = newTouchList[resultIndex].blob.centroid.y;
double xOld = touchList[j].blob.centroid.x;
double yOld = touchList[j].blob.centroid.y;
double xNew = touchList[i].blob.centroid.x;
double yNew = touchList[i].blob.centroid.y;
double distOld = (x-xOld)*(x-xOld)+(y-yOld)*(y-yOld);
double distNew = (x-xNew)*(x-xNew)+(y-yNew)*(y-yNew);
if(distNew < distOld){
newTouchList[resultIndex].id = touchList[i].id;
touchList[j].id = -1;
}
else touchList[i].id = -1;
}
}
///Трекинг прошел без конфликтов
else newTouchList[resultIndex].id = touchList[i].id;
}
}
///Шаг 3. Обновление позиций пользователей с предыдущего кадра, удаление тех, которые не были обнаружены
for(unsigned int i = 0; i < touchList.size(); i++)
{
if(touchList[i].id == -1){///Удаление пользователя
ofNotifyEvent(blobDeleted, touchList[i].blob);
touchList.erase(touchList.begin() + i, touchList.begin() + i + 1);
i--;
}
else{
for(unsigned int j = 0; j < newTouchList.size(); j++)
if(touchList[i].id == newTouchList[j].id){
///Обновление данных
ofVec3f lastCentroid = touchList[i].blob.centroid;///Центроид с предыдущего кадра
touchList[i].blob = newTouchList[j].blob;
touchList[i].framesToLive = aliveFrames;
ofVec3f positionDifference;
positionDifference.set(touchList[i].blob.centroid.x - lastCentroid.x, touchList[i].blob.centroid.y - lastCentroid.y);
///Фрагмент кода из CCV
//float posDelta = sqrtf((positionDifference.x*positionDifference.x)+(positionDifference.y*positionDifference.y));
//int MOVEMENT_FILTERING = 0;
//float a = 1.0f - 1.0f / expf(posDelta / (1.0f + (float)MOVEMENT_FILTERING*10));
//users[i].blob.centroid.x = a * users[i].blob.centroid.x + (1-a) * lastCentroid.x;
//users[i].blob.centroid.y = a * users[i].blob.centroid.y + (1-a) * lastCentroid.y;
///Конец фрагмента из CCV
/// Фильтр Калмана
if(positionDifference.length() > speedThreshold){
touchList[i].blob.centroid = touchList[i].filter.getCorrect(touchList[i].blob.centroid);
ofNotifyEvent(blobMoved, touchList[i].blob);
}
else touchList[i].blob.centroid = lastCentroid;
/*touchList[i].blob.D.set(touchList[i].blob.centroid.x - lastCentroid.x,
touchList[i].blob.centroid.y - lastCentroid.y);*/
}
}
}
///Шаг 4. Добавляем новых возможных пользователей
for(unsigned int i = 0; i < newTouchList.size(); i++){
if(newTouchList[i].id == -1){
newTouchList[i].id = idCounter;
idCounter++;
touchList.push_back(newTouchList[i]);
touchList.back().blob.id = touchList.back().id;
ofNotifyEvent(blobAdded, touchList.back().blob);
}
}
}
