root / tbeta / Windows / apps / tbeta app / Configapp / src / Tracking / tracking.cpp @ 3
View | Annotate | Download (8.1 KB)
| 1 | /*
|
|---|---|
| 2 | * tracking.cpp |
| 3 | * |
| 4 | * Created by Ramsin Khoshabeh on 5/4/08. |
| 5 | * Copyright 2008 risenparadigm. All rights reserved. |
| 6 | * |
| 7 | * Changelog: |
| 8 | * 08/15/08 -- Fixed a major bug in the track algorithm |
| 9 | */ |
| 10 | |
| 11 | #include "tracking.h" |
| 12 | |
| 13 | BlobTracker::BlobTracker() |
| 14 | {
|
| 15 | IDCounter = 0;
|
| 16 | } |
| 17 | |
| 18 | //Setup a listener
|
| 19 | void BlobTracker::setListener( ofCvBlobListener* _listener )
|
| 20 | {
|
| 21 | listener = _listener; |
| 22 | } |
| 23 | |
| 24 | |
| 25 | //assigns IDs to each blob in the contourFinder
|
| 26 | void BlobTracker::track(ofxCvContourFinder* newBlobs)
|
| 27 | {
|
| 28 | //initialize ID's of all blobs
|
| 29 | for(int i=0; i<newBlobs->nBlobs; i++) |
| 30 | newBlobs->blobs[i].id=-1;
|
| 31 | |
| 32 | //go through all tracked blobs to compute nearest new point
|
| 33 | for(int i=0; i<trackedBlobs.size(); i++) |
| 34 | {
|
| 35 | /******************************************************************
|
| 36 | * *****************TRACKING FUNCTION TO BE USED******************* |
| 37 | * Replace 'trackKnn(...)' with any function that will take the |
| 38 | * current track and find the corresponding track in the newBlobs |
| 39 | * 'winner' should contain the index of the found blob or '-1' if |
| 40 | * there was no corresponding blob |
| 41 | *****************************************************************/ |
| 42 | int winner = trackKnn(newBlobs, &(trackedBlobs[i]), 3, 0); |
| 43 | |
| 44 | if(winner==-1) //track has died, mark it for deletion |
| 45 | {
|
| 46 | //SEND BLOB OFF EVENT
|
| 47 | doBlobOff( trackedBlobs[i] ); |
| 48 | //mark the blob for deletion
|
| 49 | trackedBlobs[i].id = -1;
|
| 50 | } |
| 51 | else //still alive, have to update |
| 52 | {
|
| 53 | //if winning new blob was labeled winner by another track\
|
| 54 | //then compare with this track to see which is closer |
| 55 | if(newBlobs->blobs[winner].id!=-1) |
| 56 | {
|
| 57 | //find the currently assigned blob
|
| 58 | int j; //j will be the index of it |
| 59 | for(j=0; j<trackedBlobs.size(); j++) |
| 60 | {
|
| 61 | if(trackedBlobs[j].id==newBlobs->blobs[winner].id)
|
| 62 | break;
|
| 63 | } |
| 64 | |
| 65 | if(j==trackedBlobs.size())//got to end without finding it
|
| 66 | {
|
| 67 | newBlobs->blobs[winner].id = trackedBlobs[i].id; |
| 68 | trackedBlobs[i] = newBlobs->blobs[winner]; |
| 69 | } |
| 70 | else //found it, compare with current blob |
| 71 | {
|
| 72 | double x = newBlobs->blobs[winner].centroid.x;
|
| 73 | double y = newBlobs->blobs[winner].centroid.y;
|
| 74 | double xOld = trackedBlobs[j].centroid.x;
|
| 75 | double yOld = trackedBlobs[j].centroid.y;
|
| 76 | double xNew = trackedBlobs[i].centroid.x;
|
| 77 | double yNew = trackedBlobs[i].centroid.y;
|
| 78 | double distOld = (x-xOld)*(x-xOld)+(y-yOld)*(y-yOld);
|
| 79 | double distNew = (x-xNew)*(x-xNew)+(y-yNew)*(y-yNew);
|
| 80 | |
| 81 | //if this track is closer, update the ID of the blob
|
| 82 | //otherwise delete this track.. it's dead
|
| 83 | if(distNew<distOld) //update |
| 84 | {
|
| 85 | newBlobs->blobs[winner].id = trackedBlobs[i].id; |
| 86 | |
| 87 | //TODO--------------------------------------------------------------------------
|
| 88 | //now the old winning blob has lost the win.
|
| 89 | //I should also probably go through all the newBlobs
|
| 90 | //at the end of this loop and if there are ones without
|
| 91 | //any winning matches, check if they are close to this
|
| 92 | //one. Right now I'm not doing that to prevent a
|
| 93 | //recursive mess. It'll just be a new track.
|
| 94 | |
| 95 | //SEND BLOB OFF EVENT
|
| 96 | doBlobOff( trackedBlobs[j] ); |
| 97 | //mark the blob for deletion
|
| 98 | trackedBlobs[j].id = -1;
|
| 99 | //------------------------------------------------------------------------------
|
| 100 | } |
| 101 | else //delete |
| 102 | {
|
| 103 | //SEND BLOB OFF EVENT
|
| 104 | doBlobOff( trackedBlobs[i] ); |
| 105 | //mark the blob for deletion
|
| 106 | trackedBlobs[i].id = -1;
|
| 107 | } |
| 108 | } |
| 109 | } |
| 110 | else //no conflicts, so simply update |
| 111 | {
|
| 112 | newBlobs->blobs[winner].id = trackedBlobs[i].id; |
| 113 | } |
| 114 | } |
| 115 | } |
| 116 | |
| 117 | //--Update All Current Tracks
|
| 118 | //remove every track labeled as dead (ID='-1')
|
| 119 | //find every track that's alive and copy it's data from newBlobs
|
| 120 | for(int i=0; i<trackedBlobs.size(); i++) |
| 121 | {
|
| 122 | if(trackedBlobs[i].id==-1) //dead |
| 123 | {
|
| 124 | //erase track
|
| 125 | trackedBlobs.erase(trackedBlobs.begin()+i, |
| 126 | trackedBlobs.begin()+i+1);
|
| 127 | |
| 128 | i--; //decrement one since we removed an element
|
| 129 | } |
| 130 | else //living, so update it's data |
| 131 | {
|
| 132 | for(int j=0; j<newBlobs->nBlobs; j++) |
| 133 | {
|
| 134 | if(trackedBlobs[i].id==newBlobs->blobs[j].id)
|
| 135 | {
|
| 136 | //update track
|
| 137 | trackedBlobs[i]=newBlobs->blobs[j]; |
| 138 | |
| 139 | //SEND BLOB MOVED EVENT
|
| 140 | doBlobMoved( trackedBlobs[i] ); |
| 141 | } |
| 142 | } |
| 143 | } |
| 144 | } |
| 145 | |
| 146 | //--Add New Living Tracks
|
| 147 | //now every new blob should be either labeled with a tracked ID or\
|
| 148 | //have ID of -1... if the ID is -1... we need to make a new track |
| 149 | for(int i=0; i<newBlobs->nBlobs; i++) |
| 150 | {
|
| 151 | if(newBlobs->blobs[i].id==-1) |
| 152 | {
|
| 153 | //add new track
|
| 154 | newBlobs->blobs[i].id=IDCounter++; |
| 155 | trackedBlobs.push_back(newBlobs->blobs[i]); |
| 156 | |
| 157 | //SEND BLOB ON EVENT
|
| 158 | doBlobOn( trackedBlobs[i] ); |
| 159 | } |
| 160 | } |
| 161 | } |
| 162 | |
| 163 | |
| 164 | /*************************************************************************
|
| 165 | * Finds the blob in 'newBlobs' that is closest to the trackedBlob with index |
| 166 | * 'ind' according to the KNN algorithm and returns the index of the winner |
| 167 | * newBlobs = list of blobs detected in the latest frame |
| 168 | * track = current tracked blob being tested |
| 169 | * k = number of nearest neighbors to consider\ |
| 170 | * 1,3,or 5 are common numbers..\ |
| 171 | * must always be an odd number to avoid tying |
| 172 | * thresh = threshold for optimization |
| 173 | **************************************************************************/ |
| 174 | |
| 175 | int BlobTracker::trackKnn(ofxCvContourFinder *newBlobs, ofxCvBlob *track, int k, double thresh = 0) |
| 176 | {
|
| 177 | int winner = -1; //initially label track as '-1'=dead |
| 178 | if((k%2)==0) k++; //if k is not an odd number, add 1 to it |
| 179 | |
| 180 | //if it exists, square the threshold to use as square distance
|
| 181 | if(thresh>0) |
| 182 | thresh *= thresh; |
| 183 | |
| 184 | //list of neighbor point index and respective distances
|
| 185 | std::list<std::pair<int,double> > nbors; |
| 186 | std::list<std::pair<int,double> >::iterator iter; |
| 187 | |
| 188 | //find 'k' closest neighbors of testpoint
|
| 189 | double x, y, xT, yT, dist;
|
| 190 | for(int i=0; i<newBlobs->nBlobs; i++) |
| 191 | {
|
| 192 | x = newBlobs->blobs[i].centroid.x; |
| 193 | y = newBlobs->blobs[i].centroid.y; |
| 194 | xT = track->centroid.x; |
| 195 | yT = track->centroid.y; |
| 196 | dist = (x-xT)*(x-xT)+(y-yT)*(y-yT); |
| 197 | |
| 198 | if(dist<=thresh)//it's good, apply label if no label yet and return |
| 199 | {
|
| 200 | winner = i; |
| 201 | return winner;
|
| 202 | } |
| 203 | |
| 204 | /****************************************************************
|
| 205 | * check if this blob is closer to the point than what we've seen |
| 206 | *so far and add it to the index/distance list if positive |
| 207 | ****************************************************************/ |
| 208 | |
| 209 | //search the list for the first point with a longer distance
|
| 210 | for(iter=nbors.begin(); iter!=nbors.end()
|
| 211 | && dist>=iter->second; iter++); |
| 212 | |
| 213 | if((iter!=nbors.end())||(nbors.size()<k)) //it's valid, insert it |
| 214 | {
|
| 215 | nbors.insert(iter, 1, std::pair<int, double>(i, dist)); |
| 216 | //too many items in list, get rid of farthest neighbor
|
| 217 | if(nbors.size()>k)
|
| 218 | nbors.pop_back(); |
| 219 | } |
| 220 | } |
| 221 | |
| 222 | /********************************************************************
|
| 223 | * we now have k nearest neighbors who cast a vote, and the majority |
| 224 | * wins. we use each class average distance to the target to break any |
| 225 | * possible ties. |
| 226 | *********************************************************************/ |
| 227 | |
| 228 | // a mapping from labels (IDs) to count/distance
|
| 229 | std::map<int, std::pair<int, double> > votes; |
| 230 | |
| 231 | //remember:
|
| 232 | //iter->first = index of newBlob
|
| 233 | //iter->second = distance of newBlob to current tracked blob
|
| 234 | for(iter=nbors.begin(); iter!=nbors.end(); iter++)
|
| 235 | {
|
| 236 | //add up how many counts each neighbor got
|
| 237 | int count = ++(votes[iter->first].first);
|
| 238 | double dist = (votes[iter->first].second+=iter->second);
|
| 239 | |
| 240 | /* check for a possible tie and break with distance */
|
| 241 | if(count>votes[winner].first || count==votes[winner].first
|
| 242 | && dist<votes[winner].second) |
| 243 | {
|
| 244 | winner = iter->first; |
| 245 | } |
| 246 | } |
| 247 | |
| 248 | return winner;
|
| 249 | } |
| 250 | |
| 251 | |
| 252 | /************************************
|
| 253 | * Delegate to Callbacks |
| 254 | *************************************/ |
| 255 | void BlobTracker::doBlobOn( const ofxCvBlob& b ) |
| 256 | {
|
| 257 | if( listener != NULL ) |
| 258 | {
|
| 259 | listener->blobOn( b); |
| 260 | } else {
|
| 261 | cout << "doBlobOn() event for blob: " << b.id << endl;
|
| 262 | } |
| 263 | } |
| 264 | |
| 265 | void BlobTracker::doBlobMoved( const ofxCvBlob& b ) |
| 266 | {
|
| 267 | if( listener != NULL ) |
| 268 | {
|
| 269 | listener->blobMoved( b); |
| 270 | } else {
|
| 271 | cout << "doBlobMoved() event for blob: " << b.id << endl;
|
| 272 | } |
| 273 | } |
| 274 | |
| 275 | void BlobTracker::doBlobOff( const ofxCvBlob& b ) |
| 276 | {
|
| 277 | if( listener != NULL ) |
| 278 | {
|
| 279 | listener->blobOff( b); |
| 280 | } else {
|
| 281 | cout << "doBlobOff() event for blob: " << b.id << endl;
|
| 282 | } |
| 283 | } |
| 284 |
