root / trunk / tbeta / Windows / addons / ofxNCore / src / Tracking / Tracking.cpp

View | Annotate | Download (16.7 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
*/
12
13
#include "Tracking.h"
14
15
BlobTracker::BlobTracker()
16
{
17
        IDCounter = 0;
18
        isCalibrating = false;
19
}
20
21
BlobTracker::~BlobTracker(){
22
23
        delete calibrate;
24
}
25
26
void BlobTracker::passInCalibration(CalibrationUtils* calibrater) {
27
28
    calibrate = calibrater;
29
}
30
31
//assigns IDs to each blob in the contourFinder
32
void BlobTracker::track(ContourFinder* newBlobs)
33
{
34
        //initialize ID's of all blobs
35
        for(int i=0; i<newBlobs->nBlobs; i++)
36
                newBlobs->blobs[i].id=-1;
37
38
        //go through all tracked blobs to compute nearest new point
39
        for(int i=0; i<trackedBlobs.size(); i++)
40
        {
41
                /******************************************************************
42
                 * *****************TRACKING FUNCTION TO BE USED*******************
43
                 * Replace 'trackKnn(...)' with any function that will take the
44
                 * current track and find the corresponding track in the newBlobs
45
                 * 'winner' should contain the index of the found blob or '-1' if
46
                 * there was no corresponding blob
47
                 *****************************************************************/
48
                int winner = trackKnn(newBlobs, &(trackedBlobs[i]), 3, 0);
49
50
                if(winner==-1) //track has died, mark it for deletion
51
                {
52
                        //SEND BLOB OFF EVENT
53
                        TouchEvents.messenger = trackedBlobs[i];
54
55
                        if(isCalibrating){
56
                                TouchEvents.RAWmessenger = trackedBlobs[i];
57
                                TouchEvents.notifyRAWTouchUp(NULL);
58
                        }
59
60
                        calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
61
                        calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
62
                        //erase calibrated blob from map
63
                        calibratedBlobs.erase(TouchEvents.messenger.id);
64
65
                        TouchEvents.notifyTouchUp(NULL);
66
                        //mark the blob for deletion
67
                        trackedBlobs[i].id = -1;
68
                }
69
                else //still alive, have to update
70
                {
71
                        //if winning new blob was labeled winner by another track\
72
                        //then compare with this track to see which is closer
73
                        if(newBlobs->blobs[winner].id!=-1)
74
                        {
75
                                //find the currently assigned blob
76
                                int j; //j will be the index of it
77
                                for(j=0; j<trackedBlobs.size(); j++)
78
                                {
79
                                        if(trackedBlobs[j].id==newBlobs->blobs[winner].id)
80
                                                break;
81
                                }
82
83
                                if(j==trackedBlobs.size())//got to end without finding it
84
                                {
85
                                        newBlobs->blobs[winner].id = trackedBlobs[i].id;
86
                                        newBlobs->blobs[winner].age = trackedBlobs[i].age;
87
                                        newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
88
                                        newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
89
                                        newBlobs->blobs[winner].color = trackedBlobs[i].color;
90
                                        newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
91
92
                                        trackedBlobs[i] = newBlobs->blobs[winner];
93
                                }
94
                                else //found it, compare with current blob
95
                                {
96
                                        double x = newBlobs->blobs[winner].centroid.x;
97
                                        double y = newBlobs->blobs[winner].centroid.y;
98
                                        double xOld = trackedBlobs[j].centroid.x;
99
                                        double yOld = trackedBlobs[j].centroid.y;
100
                                        double xNew = trackedBlobs[i].centroid.x;
101
                                        double yNew = trackedBlobs[i].centroid.y;
102
                                        double distOld = (x-xOld)*(x-xOld)+(y-yOld)*(y-yOld);
103
                                        double distNew = (x-xNew)*(x-xNew)+(y-yNew)*(y-yNew);
104
105
                                        //if this track is closer, update the ID of the blob
106
                                        //otherwise delete this track.. it's dead
107
                                        if(distNew<distOld) //update
108
                                        {
109
                                                newBlobs->blobs[winner].id = trackedBlobs[i].id;
110
                                                newBlobs->blobs[winner].age = trackedBlobs[i].age;
111
                                                newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
112
                                                newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
113
                                                newBlobs->blobs[winner].color = trackedBlobs[i].color;
114
                                                newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
115
116
//TODO--------------------------------------------------------------------------
117
                                                //now the old winning blob has lost the win.
118
                                                //I should also probably go through all the newBlobs
119
                                                //at the end of this loop and if there are ones without
120
                                                //any winning matches, check if they are close to this
121
                                                //one. Right now I'm not doing that to prevent a
122
                                                //recursive mess. It'll just be a new track.
123
124
                                                //SEND BLOB OFF EVENT
125
                                                TouchEvents.messenger = trackedBlobs[j];
126
127
                                                if(isCalibrating){
128
                                                        TouchEvents.RAWmessenger = trackedBlobs[j];
129
                                                        TouchEvents.notifyRAWTouchUp(NULL);
130
                                                }
131
132
                        calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
133
                        calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
134
                                                //erase calibrated blob from map
135
                                                calibratedBlobs.erase(TouchEvents.messenger.id);
136
137
                                             TouchEvents.notifyTouchUp(NULL);
138
                                                //mark the blob for deletion
139
                                                trackedBlobs[j].id = -1;
140
//------------------------------------------------------------------------------
141
                                        }
142
                                        else //delete
143
                                        {
144
                                                //SEND BLOB OFF EVENT
145
                                                TouchEvents.messenger = trackedBlobs[i];
146
147
                                                if(isCalibrating){
148
                                                        TouchEvents.RAWmessenger = trackedBlobs[i];
149
                                                        TouchEvents.notifyRAWTouchUp(NULL);
150
                                                }
151
152
                        calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
153
                        calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
154
                                                //erase calibrated blob from map
155
                                                calibratedBlobs.erase(TouchEvents.messenger.id);
156
157
                                                TouchEvents.notifyTouchUp(NULL);
158
                                                //mark the blob for deletion
159
                                                trackedBlobs[i].id = -1;
160
                                        }
161
                                }
162
                        }
163
                        else //no conflicts, so simply update
164
                        {
165
                                newBlobs->blobs[winner].id = trackedBlobs[i].id;
166
                                newBlobs->blobs[winner].age = trackedBlobs[i].age;
167
                                newBlobs->blobs[winner].sitting = trackedBlobs[i].sitting;
168
                                newBlobs->blobs[winner].downTime = trackedBlobs[i].downTime;
169
                                newBlobs->blobs[winner].color = trackedBlobs[i].color;
170
                                newBlobs->blobs[winner].lastTimeTimeWasChecked = trackedBlobs[i].lastTimeTimeWasChecked;
171
                        }
172
                }
173
        }
174
175
        //--Update All Current Tracks
176
        //remove every track labeled as dead (ID='-1')
177
        //find every track that's alive and copy it's data from newBlobs
178
        for(int i=0; i<trackedBlobs.size(); i++)
179
        {
180
                if(trackedBlobs[i].id==-1) //dead
181
                {
182
                        //erase track
183
                        trackedBlobs.erase(trackedBlobs.begin()+i,
184
                                                           trackedBlobs.begin()+i+1);
185
186
                        i--; //decrement one since we removed an element
187
                }
188
                else //living, so update it's data
189
                {
190
                        for(int j=0; j<newBlobs->nBlobs; j++)
191
                        {
192
                                if(trackedBlobs[i].id==newBlobs->blobs[j].id)
193
                                {
194
                                        //update track
195
                                        ofPoint tempLastCentroid = trackedBlobs[i].centroid; // assign the new centroid to the old
196
                                        trackedBlobs[i]=newBlobs->blobs[j];
197
                                        trackedBlobs[i].lastCentroid = tempLastCentroid;
198
199
                                        //get the Differences in position
200
                                        trackedBlobs[i].D.set((trackedBlobs[i].centroid.x - trackedBlobs[i].lastCentroid.x) / (ofGetElapsedTimeMillis() - trackedBlobs[i].lastTimeTimeWasChecked), 
201
                                                                                  (trackedBlobs[i].centroid.y - trackedBlobs[i].lastCentroid.y) / (ofGetElapsedTimeMillis() - trackedBlobs[i].lastTimeTimeWasChecked));
202
203
                                        //printf("D(%f, %f)\n", trackedBlobs[i].D.x, trackedBlobs[i].D.y);
204
                                        
205
                                        //if( abs((int)trackedBlobs[i].D.x) > 1 || abs((int)trackedBlobs[i].D.y) > 1) {
206
//                                                printf("\nUNUSUAL BLOB @ %f\n-----------------------\ntrackedBlobs[%i]\nD = (%f, %f)\nXY= (%f, %f)\nlastTimeTimeWasChecked = %f\nsitting = %f\n",
207
//                                                           ofGetElapsedTimeMillis(),
208
//                                                           i,
209
//                                                           trackedBlobs[i].D.x,  trackedBlobs[i].D.y,
210
//                                                           trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y,
211
//                                                           trackedBlobs[i].lastTimeTimeWasChecked,
212
//                                                           trackedBlobs[i].downTime,
213
//                                                           trackedBlobs[i].sitting
214
//                                                );
215
//                                        }
216
                                        
217
                                        
218
                                        //calculate the accelleration
219
                                        ofPoint tD = trackedBlobs[i].D;
220
                                        trackedBlobs[i].maccel = sqrtf((tD.x* tD.x)+(tD.y*tD.y)/(ofGetElapsedTimeMillis() - trackedBlobs[i].lastTimeTimeWasChecked));
221
                                        
222
                                        trackedBlobs[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
223
224
                                        //calculate the age
225
                                        trackedBlobs[i].age = ofGetElapsedTimef() - trackedBlobs[i].downTime;
226
227
                                        //if not moving more than min_movement_threshold then set to same position as last frame
228
                            if(trackedBlobs[i].maccel < MIN_MOVEMENT_THRESHOLD)
229
                                        {        //this helps avoid jittery blobs
230
                                                trackedBlobs[i].centroid.x = trackedBlobs[i].lastCentroid.x;
231
                                                trackedBlobs[i].centroid.y = trackedBlobs[i].lastCentroid.y;
232
                    }
233
234
                                        //set sitting (held length)
235
                    if(trackedBlobs[i].maccel < 7)
236
                                        {        //1 more frame of sitting
237
                                                if(trackedBlobs[i].sitting != -1)
238
                                                trackedBlobs[i].sitting = ofGetElapsedTimef() - trackedBlobs[i].downTime;           
239
                                        }
240
                                        else {
241
                                                trackedBlobs[i].sitting = -1;
242
                                        }
243
244
                                        //printf("time: %f\n", ofGetElapsedTimeMillis());
245
                                        //printf("%i age: %f, downTimed at: %f\n", i, trackedBlobs[i].age, trackedBlobs[i].downTime);
246
247
                                        //if blob has been 'holding/sitting' for 1 second send a held event
248
                                        if(trackedBlobs[i].sitting > 1.0f){
249
250
                                                //SEND BLOB HELD EVENT
251
                                                TouchEvents.messenger = trackedBlobs[i];
252
253
                                                if(isCalibrating){
254
                                                        TouchEvents.RAWmessenger = trackedBlobs[i];
255
                                                        TouchEvents.notifyRAWTouchHeld(NULL);
256
                                                }
257
                                                
258
                                                //calibrated values
259
                                                calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
260
                                                calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
261
                                                calibrate->cameraToScreenPosition(TouchEvents.messenger.lastCentroid.x, TouchEvents.messenger.lastCentroid.y);
262
                                                
263
                                                //Calibrated dx/dy
264
                                                TouchEvents.messenger.D.set((TouchEvents.messenger.centroid.x - TouchEvents.messenger.lastCentroid.x) / (ofGetElapsedTimeMillis() - TouchEvents.messenger.lastTimeTimeWasChecked), 
265
                                                                                                        (TouchEvents.messenger.centroid.y - TouchEvents.messenger.lastCentroid.y) / (ofGetElapsedTimeMillis() - TouchEvents.messenger.lastTimeTimeWasChecked));
266
                                                
267
                                                TouchEvents.messenger.lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
268
                                                
269
                                                
270
271
                                                
272
                                                //calibrated accelleration
273
                                                ofPoint tD2 = TouchEvents.messenger.D;
274
                                                TouchEvents.messenger.maccel = sqrtf((tD2.x* tD2.x)+(tD2.y*tD2.y)/(ofGetElapsedTimeMillis() - trackedBlobs[i].lastTimeTimeWasChecked));                                                
275
                                                
276
                                                //add to calibration map
277
                                                calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
278
279
                        //held event only happens once so set to -1
280
                        trackedBlobs[i].sitting = -1;
281
282
                                                TouchEvents.notifyTouchHeld(NULL);
283
284
                                        } else {
285
                                                
286
                                                //printf("(%f, %f) -> (%f, %f) \n", trackedBlobs[i].lastCentroid.x, trackedBlobs[i].lastCentroid.y, trackedBlobs[i].centroid.x, trackedBlobs[i].centroid.y);
287
288
                                                //SEND BLOB MOVED EVENT
289
                                                TouchEvents.messenger = trackedBlobs[i];
290
291
                                                if(isCalibrating){
292
                                                        TouchEvents.RAWmessenger = trackedBlobs[i];
293
                                                        TouchEvents.notifyRAWTouchMoved(NULL);
294
                                                }
295
296
                                                //calibrated values
297
                                                calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
298
                                                calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
299
                                                calibrate->cameraToScreenPosition(TouchEvents.messenger.lastCentroid.x, TouchEvents.messenger.lastCentroid.y);
300
                                                
301
                                                //Calibrated dx/dy
302
                                                TouchEvents.messenger.D.set((TouchEvents.messenger.centroid.x - TouchEvents.messenger.lastCentroid.x) / (ofGetElapsedTimeMillis() - TouchEvents.messenger.lastTimeTimeWasChecked), 
303
                                                                                                        (TouchEvents.messenger.centroid.y - TouchEvents.messenger.lastCentroid.y) / (ofGetElapsedTimeMillis() - TouchEvents.messenger.lastTimeTimeWasChecked));
304
        
305
                                                
306
                                                TouchEvents.messenger.lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
307
                                                
308
                                                
309
                                                //printf("d(%0.4f, %0.4f)\n", TouchEvents.messenger.D.x, TouchEvents.messenger.D.y);
310
311
                                                
312
                                                                                                
313
                                                //calibrated accelleration
314
                                                ofPoint tD2 = TouchEvents.messenger.D;
315
                                                TouchEvents.messenger.maccel = sqrtf((tD2.x* tD2.x)+(tD2.y*tD2.y)/(ofGetElapsedTimeMillis() - trackedBlobs[i].lastTimeTimeWasChecked));                                                
316
                                                
317
                                                //add to calibration map
318
                                                calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
319
320
                                                TouchEvents.notifyTouchMoved(NULL);
321
                                        }
322
                                }
323
                        }
324
                }
325
        }
326
        //--Add New Living Tracks
327
        //now every new blob should be either labeled with a tracked ID or\
328
        //have ID of -1... if the ID is -1... we need to make a new track
329
        for(int i=0; i<newBlobs->nBlobs; i++)
330
        {
331
                if(newBlobs->blobs[i].id==-1)
332
                {
333
                        //add new track
334
                        newBlobs->blobs[i].id=IDCounter++;
335
                        newBlobs->blobs[i].downTime = ofGetElapsedTimef();
336
                        //newBlobs->blobs[i].lastTimeTimeWasChecked = ofGetElapsedTimeMillis();
337
338
                        //random color for blob. Could be useful?
339
                        int r = ofRandom(0, 255);
340
            int g = ofRandom(0, 255);
341
            int b = ofRandom(0, 255);
342
            //Convert to hex
343
            int rgbNum = ((r & 0xff) << 16) | ((g & 0xff) << 8) | (b & 0xff);
344
            //Set color
345
            newBlobs->blobs[i].color = rgbNum;
346
347
                        //Add to blob messenger
348
                        TouchEvents.messenger = newBlobs->blobs[i];
349
350
                        if(isCalibrating){
351
                                TouchEvents.RAWmessenger = newBlobs->blobs[i];
352
                                TouchEvents.notifyRAWTouchDown(NULL);
353
                        }
354
355
            calibrate->transformDimension(TouchEvents.messenger.boundingRect.width, TouchEvents.messenger.boundingRect.height);
356
            calibrate->cameraToScreenPosition(TouchEvents.messenger.centroid.x, TouchEvents.messenger.centroid.y);
357
                        //add to calibrated blob map
358
                        calibratedBlobs[TouchEvents.messenger.id] = TouchEvents.messenger;
359
360
                        //Send Event
361
                        TouchEvents.notifyTouchDown(NULL);
362
363
                        trackedBlobs.push_back(newBlobs->blobs[i]);
364
                }
365
        }
366
}
367
368
std::map<int, Blob> BlobTracker::getTrackedBlobs(){
369
        
370
    return calibratedBlobs;
371
}
372
373
/*************************************************************************
374
* Finds the blob in 'newBlobs' that is closest to the trackedBlob with index
375
* 'ind' according to the KNN algorithm and returns the index of the winner
376
* newBlobs        = list of blobs detected in the latest frame
377
* track                = current tracked blob being tested
378
* k                        = number of nearest neighbors to consider\
379
*                          1,3,or 5 are common numbers..\
380
*                          must always be an odd number to avoid tying
381
* thresh        = threshold for optimization
382
**************************************************************************/
383
384
int BlobTracker::trackKnn(ContourFinder *newBlobs, Blob *track, int k, double thresh = 0)
385
{
386
387
        int winner = -1; //initially label track as '-1'=dead
388
        if((k%2)==0) k++; //if k is not an odd number, add 1 to it
389
390
        //if it exists, square the threshold to use as square distance
391
        if(thresh>0)
392
                thresh *= thresh;
393
394
        //list of neighbor point index and respective distances
395
        std::list<std::pair<int,double> > nbors;
396
        std::list<std::pair<int,double> >::iterator iter;
397
398
        //find 'k' closest neighbors of testpoint
399
        double x, y, xT, yT, dist;
400
        for(int i=0; i<newBlobs->nBlobs; i++)
401
        {
402
                x = newBlobs->blobs[i].centroid.x;
403
                y = newBlobs->blobs[i].centroid.y;
404
405
                xT = track->centroid.x;
406
                yT = track->centroid.y;
407
                dist = (x-xT)*(x-xT)+(y-yT)*(y-yT);
408
409
                if(dist<=thresh)//it's good, apply label if no label yet and return
410
                {
411
                        winner = i;
412
                        return winner;
413
                }
414
415
                /****************************************************************
416
                * check if this blob is closer to the point than what we've seen
417
                *so far and add it to the index/distance list if positive
418
                ****************************************************************/
419
420
                //search the list for the first point with a longer distance
421
                for(iter=nbors.begin(); iter!=nbors.end()
422
                        && dist>=iter->second; iter++);
423
424
                if((iter!=nbors.end())||(nbors.size()<k)) //it's valid, insert it
425
                {
426
                        nbors.insert(iter, 1, std::pair<int, double>(i, dist));
427
                        //too many items in list, get rid of farthest neighbor
428
                        if(nbors.size()>k)
429
                                nbors.pop_back();
430
                }
431
        }
432
433
        /********************************************************************
434
        * we now have k nearest neighbors who cast a vote, and the majority
435
        * wins. we use each class average distance to the target to break any
436
        * possible ties.
437
        *********************************************************************/
438
439
        // a mapping from labels (IDs) to count/distance
440
        std::map<int, std::pair<int, double> > votes;
441
442
        //remember:
443
        //iter->first = index of newBlob
444
        //iter->second = distance of newBlob to current tracked blob
445
        for(iter=nbors.begin(); iter!=nbors.end(); iter++)
446
        {
447
                //add up how many counts each neighbor got
448
                int count = ++(votes[iter->first].first);
449
                double dist = (votes[iter->first].second+=iter->second);
450
451
                /* check for a possible tie and break with distance */
452
                if(count>votes[winner].first || count==votes[winner].first
453
                        && dist<votes[winner].second)
454
                {
455
                        winner = iter->first;
456
                }
457
        }
458
459
        return winner;
460
}