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