modules/contrib/src/detection_based_tracker.cpp

   1 #if defined(__linux__) || defined(LINUX) || defined(__APPLE__) || defined(ANDROID)
   2 #include "opencv2/contrib/detection_based_tracker.hpp"
   3
   4 #define DEBUGLOGS 1
   5
   6 #if ANDROID
   7 #include <android/log.h>
   8 #define LOG_TAG "OBJECT_DETECTOR"
   9 #define LOGD0(...) ((void)__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__))
  10 #define LOGI0(...) ((void)__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__))
  11 #define LOGW0(...) ((void)__android_log_print(ANDROID_LOG_WARN, LOG_TAG, __VA_ARGS__))
  12 #define LOGE0(...) ((void)__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__))
  13 #else
  14
  15 #include <stdio.h>
  16
  17 #define LOGD0(_str, ...) do{printf(_str , ## __VA_ARGS__); printf("\n");fflush(stdout);} while(0)
  18 #define LOGI0(_str, ...) do{printf(_str , ## __VA_ARGS__); printf("\n");fflush(stdout);} while(0)
  19 #define LOGW0(_str, ...) do{printf(_str , ## __VA_ARGS__); printf("\n");fflush(stdout);} while(0)
  20 #define LOGE0(_str, ...) do{printf(_str , ## __VA_ARGS__); printf("\n");fflush(stdout);} while(0)
  21 #endif
  22
  23 #if DEBUGLOGS
  24 #define LOGD(_str, ...) LOGD0(_str , ## __VA_ARGS__)
  25 #define LOGI(_str, ...) LOGI0(_str , ## __VA_ARGS__)
  26 #define LOGW(_str, ...) LOGW0(_str , ## __VA_ARGS__)
  27 #define LOGE(_str, ...) LOGE0(_str , ## __VA_ARGS__)
  28 #else
  29 #define LOGD(...) do{} while(0)
  30 #define LOGI(...) do{} while(0)
  31 #define LOGW(...) do{} while(0)
  32 #define LOGE(...) do{} while(0)
  33 #endif
  34
  35
  36 using namespace cv;
  37 using namespace std;
  38
  39 static inline cv::Point2f centerRect(const cv::Rect& r)
  40 {
  41     return cv::Point2f(r.x+((float)r.width)/2, r.y+((float)r.height)/2);
  42 };
  43 static inline cv::Rect scale_rect(const cv::Rect& r, float scale)
  44 {
  45     cv::Point2f m=centerRect(r);
  46     float width  = r.width  * scale;
  47     float height = r.height * scale;
  48     int x=cvRound(m.x - width/2);
  49     int y=cvRound(m.y - height/2);
  50
  51     return cv::Rect(x, y, cvRound(width), cvRound(height));
  52 };
  53
  54 void* workcycleObjectDetectorFunction(void* p);
  55 class DetectionBasedTracker::SeparateDetectionWork
  56 {
  57     public:
  58         SeparateDetectionWork(DetectionBasedTracker& _detectionBasedTracker, const std::string& cascadeFilename);
  59         virtual ~SeparateDetectionWork();
  60         bool communicateWithDetectingThread(const Mat& imageGray, vector<Rect>& rectsWhereRegions);
  61         bool run();
  62         void stop();
  63         void resetTracking();
  64         inline bool isWorking()
  65         {
  66             return (stateThread==STATE_THREAD_WORKING_SLEEPING) || (stateThread==STATE_THREAD_WORKING_WITH_IMAGE);
  67         }
  68         inline void lock()
  69         {
  70             pthread_mutex_lock(&mutex);
  71         }
  72         inline void unlock()
  73         {
  74             pthread_mutex_unlock(&mutex);
  75         }
  76
  77     protected:
  78
  79         DetectionBasedTracker& detectionBasedTracker;
  80         cv::CascadeClassifier cascadeInThread;
  81
  82         pthread_t second_workthread;
  83         pthread_mutex_t mutex;
  84         pthread_cond_t objectDetectorRun;
  85         pthread_cond_t objectDetectorThreadStartStop;
  86
  87         std::vector<cv::Rect> resultDetect;
  88         volatile bool isObjectDetectingReady;
  89         volatile bool shouldObjectDetectingResultsBeForgot;
  90
  91         enum StateSeparatedThread {
  92             STATE_THREAD_STOPPED=0,
  93             STATE_THREAD_WORKING_SLEEPING,
  94             STATE_THREAD_WORKING_WITH_IMAGE,
  95             STATE_THREAD_WORKING,
  96             STATE_THREAD_STOPPING
  97         };
  98         volatile StateSeparatedThread stateThread;
  99
 100         cv::Mat imageSeparateDetecting;
 101
 102         void workcycleObjectDetector();
 103         friend void* workcycleObjectDetectorFunction(void* p);
 104
 105         long long  timeWhenDetectingThreadStartedWork;
 106 };
 107
 108 DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork(DetectionBasedTracker& _detectionBasedTracker, const std::string& cascadeFilename)
 109     :detectionBasedTracker(_detectionBasedTracker),
 110     cascadeInThread(cascadeFilename),
 111     isObjectDetectingReady(false),
 112     shouldObjectDetectingResultsBeForgot(false),
 113     stateThread(STATE_THREAD_STOPPED),
 114     timeWhenDetectingThreadStartedWork(-1)
 115 {
 116     int res=0;
 117     res=pthread_mutex_init(&mutex, NULL);//TODO: should be attributes?
 118     if (res) {
 119         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_mutex_init(&mutex, NULL) is %d", res);
 120         throw(std::exception());
 121     }
 122     res=pthread_cond_init (&objectDetectorRun, NULL);
 123     if (res) {
 124         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_cond_init(&objectDetectorRun,, NULL) is %d", res);
 125         pthread_mutex_destroy(&mutex);
 126         throw(std::exception());
 127     }
 128     res=pthread_cond_init (&objectDetectorThreadStartStop, NULL);
 129     if (res) {
 130         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_cond_init(&objectDetectorThreadStartStop,, NULL) is %d", res);
 131         pthread_cond_destroy(&objectDetectorRun);
 132         pthread_mutex_destroy(&mutex);
 133         throw(std::exception());
 134     }
 135 }
 136
 137 DetectionBasedTracker::SeparateDetectionWork::~SeparateDetectionWork()
 138 {
 139     if(stateThread!=STATE_THREAD_STOPPED) {
 140         LOGE("\n\n\nATTENTION!!! dangerous algorithm error: destructor DetectionBasedTracker::DetectionBasedTracker::~SeparateDetectionWork is called before stopping the workthread");
 141     }
 142
 143     pthread_cond_destroy(&objectDetectorThreadStartStop);
 144     pthread_cond_destroy(&objectDetectorRun);
 145     pthread_mutex_destroy(&mutex);
 146 }
 147 bool DetectionBasedTracker::SeparateDetectionWork::run()
 148 {
 149     LOGD("DetectionBasedTracker::SeparateDetectionWork::run() --- start");
 150     pthread_mutex_lock(&mutex);
 151     if (stateThread != STATE_THREAD_STOPPED) {
 152         LOGE("DetectionBasedTracker::SeparateDetectionWork::run is called while the previous run is not stopped");
 153         pthread_mutex_unlock(&mutex);
 154         return false;
 155     }
 156     stateThread=STATE_THREAD_WORKING_SLEEPING;
 157     pthread_create(&second_workthread, NULL, workcycleObjectDetectorFunction, (void*)this); //TODO: add attributes?
 158     pthread_cond_wait(&objectDetectorThreadStartStop, &mutex);
 159     pthread_mutex_unlock(&mutex);
 160     LOGD("DetectionBasedTracker::SeparateDetectionWork::run --- end");
 161     return true;
 162 }
 163
 164 #ifdef __GNUC__
 165 #define CATCH_ALL_AND_LOG(_block)                                                       \
 166 do {                                                                               \
 167     try {                                                                                   \
 168         _block;                                                                             \
 169         break;                                                                              \
 170     }                                                                                       \
 171     catch(cv::Exception& e) {                                                               \
 172         LOGE0("\n %s: ERROR: OpenCV Exception caught: \n'%s'\n\n", __func__, e.what());      \
 173     } catch(std::exception& e) {                                                            \
 174         LOGE0("\n %s: ERROR: Exception caught: \n'%s'\n\n", __func__, e.what());             \
 175     } catch(...) {                                                                          \
 176         LOGE0("\n %s: ERROR: UNKNOWN Exception caught\n\n", __func__);                       \
 177     }                                                                                       \
 178 } while(0)
 179 #else
 180 #define CATCH_ALL_AND_LOG(_block)                                                       \
 181 do {                                                                               \
 182     try {                                                                                   \
 183         _block;                                                                             \
 184         break;                                                                              \
 185     }                                                                                       \
 186     catch(cv::Exception& e) {                                                               \
 187         LOGE0("\n ERROR: OpenCV Exception caught: \n'%s'\n\n", e.what());                    \
 188     } catch(std::exception& e) {                                                            \
 189         LOGE0("\n ERROR: Exception caught: \n'%s'\n\n", e.what());                           \
 190     } catch(...) {                                                                          \
 191         LOGE0("\n ERROR: UNKNOWN Exception caught\n\n");                                     \
 192     }                                                                                       \
 193 } while(0)
 194 #endif
 195
 196 void* workcycleObjectDetectorFunction(void* p)
 197 {
 198     CATCH_ALL_AND_LOG({ ((DetectionBasedTracker::SeparateDetectionWork*)p)->workcycleObjectDetector(); });
 199     try{
 200         ((DetectionBasedTracker::SeparateDetectionWork*)p)->stateThread=DetectionBasedTracker::SeparateDetectionWork::STATE_THREAD_STOPPED;
 201     } catch(...) {
 202         LOGE0("DetectionBasedTracker: workcycleObjectDetectorFunction: ERROR concerning pointer, received as the function parameter");
 203     }
 204     return NULL;
 205 }
 206
 207 void DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector()
 208 {
 209     static double freq = getTickFrequency();
 210     LOGD0("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- start");
 211     vector<Rect> objects;
 212
 213     CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
 214     pthread_mutex_lock(&mutex);
 215     {
 216         pthread_cond_signal(&objectDetectorThreadStartStop);
 217
 218         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- before waiting");
 219         CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
 220         pthread_cond_wait(&objectDetectorRun, &mutex);
 221         if (isWorking()) {
 222             stateThread=STATE_THREAD_WORKING_WITH_IMAGE;
 223         }
 224         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- after waiting");
 225     }
 226     pthread_mutex_unlock(&mutex);
 227
 228     bool isFirstStep=true;
 229
 230     isObjectDetectingReady=false;
 231
 232     while(isWorking())
 233     {
 234         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- next step");
 235
 236         if (! isFirstStep) {
 237             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- before waiting");
 238             CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
 239
 240             pthread_mutex_lock(&mutex);
 241             CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
 242             pthread_cond_wait(&objectDetectorRun, &mutex);
 243             if (isWorking()) {
 244                 stateThread=STATE_THREAD_WORKING_WITH_IMAGE;
 245             }
 246             pthread_mutex_unlock(&mutex);
 247
 248             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- after waiting");
 249         } else {
 250             isFirstStep=false;
 251         }
 252
 253         if (!isWorking()) {
 254             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- go out from the workcycle just after waiting");
 255             break;
 256         }
 257
 258
 259         if (imageSeparateDetecting.empty()) {
 260             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- imageSeparateDetecting is empty, continue");
 261             continue;
 262         }
 263         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- start handling imageSeparateDetecting, img.size=%dx%d, img.data=0x%p",
 264                 imageSeparateDetecting.size().width, imageSeparateDetecting.size().height, (void*)imageSeparateDetecting.data);
 265
 266
 267         int64 t1_detect=getTickCount();
 268
 269         int minObjectSize=detectionBasedTracker.parameters.minObjectSize;
 270         Size min_objectSize=Size(minObjectSize, minObjectSize);
 271
 272         int maxObjectSize=detectionBasedTracker.parameters.maxObjectSize;
 273         Size max_objectSize(maxObjectSize, maxObjectSize);
 274
 275
 276         cascadeInThread.detectMultiScale( imageSeparateDetecting, objects,
 277                 detectionBasedTracker.parameters.scaleFactor, detectionBasedTracker.parameters.minNeighbors, 0
 278                 |CV_HAAR_SCALE_IMAGE
 279                 ,
 280                 min_objectSize,
 281                 max_objectSize
 282                 );
 283         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- end handling imageSeparateDetecting");
 284
 285         if (!isWorking()) {
 286             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- go out from the workcycle just after detecting");
 287             break;
 288         }
 289
 290         int64 t2_detect=getTickCount();
 291         int64 dt_detect=t2_detect-t1_detect;
 292         double dt_detect_ms=((double)dt_detect)/freq * 1000.0;
 293
 294         LOGI("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- objects num==%d, t_ms=%.4f", (int)objects.size(), dt_detect_ms);
 295
 296         pthread_mutex_lock(&mutex);
 297         if (!shouldObjectDetectingResultsBeForgot) {
 298             resultDetect=objects;
 299             isObjectDetectingReady=true;
 300         } else { //shouldObjectDetectingResultsBeForgot==true
 301             resultDetect.clear();
 302             isObjectDetectingReady=false;
 303             shouldObjectDetectingResultsBeForgot=false;
 304         }
 305         if(isWorking()) {
 306             stateThread=STATE_THREAD_WORKING_SLEEPING;
 307         }
 308         pthread_mutex_unlock(&mutex);
 309
 310         objects.clear();
 311     }// while(isWorking())
 312
 313
 314     pthread_mutex_lock(&mutex);
 315
 316     stateThread=STATE_THREAD_STOPPED;
 317
 318     isObjectDetectingReady=false;
 319     shouldObjectDetectingResultsBeForgot=false;
 320
 321     pthread_cond_signal(&objectDetectorThreadStartStop);
 322
 323     pthread_mutex_unlock(&mutex);
 324
 325     LOGI("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector: Returning");
 326 }
 327
 328 void DetectionBasedTracker::SeparateDetectionWork::stop()
 329 {
 330     //FIXME: TODO: should add quickStop functionality
 331     pthread_mutex_lock(&mutex);
 332     if (!isWorking()) {
 333         pthread_mutex_unlock(&mutex);
 334         LOGE("SimpleHighguiDemoCore::stop is called but the SimpleHighguiDemoCore pthread is not active");
 335         return;
 336     }
 337     stateThread=STATE_THREAD_STOPPING;
 338     LOGD("DetectionBasedTracker::SeparateDetectionWork::stop: before going to sleep to wait for the signal from the workthread");
 339     pthread_cond_signal(&objectDetectorRun);
 340     pthread_cond_wait(&objectDetectorThreadStartStop, &mutex);
 341     LOGD("DetectionBasedTracker::SeparateDetectionWork::stop: after receiving the signal from the workthread, stateThread=%d", (int)stateThread);
 342     pthread_mutex_unlock(&mutex);
 343 }
 344
 345 void DetectionBasedTracker::SeparateDetectionWork::resetTracking()
 346 {
 347     LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking");
 348     pthread_mutex_lock(&mutex);
 349
 350     if (stateThread == STATE_THREAD_WORKING_WITH_IMAGE) {
 351         LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking: since workthread is detecting objects at the moment, we should make cascadeInThread stop detecting and forget the detecting results");
 352         shouldObjectDetectingResultsBeForgot=true;
 353         //cascadeInThread.setStopFlag();//FIXME: TODO: this feature also should be contributed to OpenCV
 354     } else {
 355         LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking: since workthread is NOT detecting objects at the moment, we should NOT make any additional actions");
 356     }
 357
 358     resultDetect.clear();
 359     isObjectDetectingReady=false;
 360
 361
 362     pthread_mutex_unlock(&mutex);
 363
 364 }
 365
 366 bool DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread(const Mat& imageGray, vector<Rect>& rectsWhereRegions)
 367 {
 368     static double freq = getTickFrequency();
 369
 370     bool shouldCommunicateWithDetectingThread=(stateThread==STATE_THREAD_WORKING_SLEEPING);
 371     LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: shouldCommunicateWithDetectingThread=%d", (shouldCommunicateWithDetectingThread?1:0));
 372
 373     if (!shouldCommunicateWithDetectingThread) {
 374         return false;
 375     }
 376
 377     bool shouldHandleResult=false;
 378     pthread_mutex_lock(&mutex);
 379
 380     if (isObjectDetectingReady) {
 381         shouldHandleResult=true;
 382         rectsWhereRegions=resultDetect;
 383         isObjectDetectingReady=false;
 384
 385         double lastBigDetectionDuration=1000.0 * (((double)(getTickCount()  - timeWhenDetectingThreadStartedWork )) / freq);
 386         LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: lastBigDetectionDuration=%f ms", (double)lastBigDetectionDuration);
 387     }
 388
 389     bool shouldSendNewDataToWorkThread=true;
 390     if (timeWhenDetectingThreadStartedWork > 0) {
 391         double time_from_previous_launch_in_ms=1000.0 * (((double)(getTickCount()  - timeWhenDetectingThreadStartedWork )) / freq); //the same formula as for lastBigDetectionDuration
 392         shouldSendNewDataToWorkThread = (time_from_previous_launch_in_ms >= detectionBasedTracker.parameters.minDetectionPeriod);
 393         LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: shouldSendNewDataToWorkThread was 1, now it is %d, since time_from_previous_launch_in_ms=%.2f, minDetectionPeriod=%d",
 394                 (shouldSendNewDataToWorkThread?1:0), time_from_previous_launch_in_ms, detectionBasedTracker.parameters.minDetectionPeriod);
 395     }
 396
 397     if (shouldSendNewDataToWorkThread) {
 398
 399         imageSeparateDetecting.create(imageGray.size(), CV_8UC1);
 400
 401         imageGray.copyTo(imageSeparateDetecting);//may change imageSeparateDetecting ptr. But should not.
 402
 403
 404         timeWhenDetectingThreadStartedWork = getTickCount() ;
 405
 406         pthread_cond_signal(&objectDetectorRun);
 407     }
 408
 409     pthread_mutex_unlock(&mutex);
 410     LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: result: shouldHandleResult=%d", (shouldHandleResult?1:0));
 411
 412     return shouldHandleResult;
 413 }
 414
 415 DetectionBasedTracker::Parameters::Parameters()
 416 {
 417     minObjectSize=96;
 418     maxObjectSize=INT_MAX;
 419     scaleFactor=1.1;
 420     maxTrackLifetime=5;
 421     minNeighbors=2;
 422     minDetectionPeriod=0;
 423 }
 424
 425
 426
 427 DetectionBasedTracker::InnerParameters::InnerParameters()
 428 {
 429     numLastPositionsToTrack=4;
 430     numStepsToWaitBeforeFirstShow=6;
 431     numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown=3;
 432     numStepsToShowWithoutDetecting=3;
 433
 434     coeffTrackingWindowSize=2.0;
 435     coeffObjectSizeToTrack=0.85;
 436     coeffObjectSpeedUsingInPrediction=0.8;
 437
 438 }
 439 DetectionBasedTracker::DetectionBasedTracker(const std::string& cascadeFilename, const Parameters& params)
 440     :separateDetectionWork(),
 441     innerParameters(),
 442     numTrackedSteps(0),
 443     cascadeForTracking(cascadeFilename)
 444 {
 445     CV_Assert( (params.minObjectSize > 0)
 446             && (params.maxObjectSize >= 0)
 447             && (params.scaleFactor > 1.0)
 448             && (params.maxTrackLifetime >= 0) );
 449
 450     parameters=params;
 451
 452     separateDetectionWork=new SeparateDetectionWork(*this, cascadeFilename);
 453
 454     weightsPositionsSmoothing.push_back(1);
 455     weightsSizesSmoothing.push_back(0.5);
 456     weightsSizesSmoothing.push_back(0.3);
 457     weightsSizesSmoothing.push_back(0.2);
 458
 459 }
 460 DetectionBasedTracker::~DetectionBasedTracker()
 461 {
 462 }
 463
 464
 465
 466 void DetectionBasedTracker::process(const Mat& imageGray)
 467 {
 468
 469     CV_Assert(imageGray.type()==CV_8UC1);
 470
 471     if (!separateDetectionWork->isWorking()) {
 472         separateDetectionWork->run();
 473     }
 474
 475     static double freq = getTickFrequency();
 476     static long long time_when_last_call_started=getTickCount();
 477
 478     {
 479         double delta_time_from_prev_call=1000.0 * (((double)(getTickCount()  - time_when_last_call_started)) / freq);
 480         LOGD("DetectionBasedTracker::process: time from the previous call is %f ms", (double)delta_time_from_prev_call);
 481         time_when_last_call_started=getTickCount();
 482     }
 483
 484     Mat imageDetect=imageGray;
 485
 486     int D=parameters.minObjectSize;
 487     if (D < 1)
 488         D=1;
 489
 490     vector<Rect> rectsWhereRegions;
 491     bool shouldHandleResult=separateDetectionWork->communicateWithDetectingThread(imageGray, rectsWhereRegions);
 492
 493
 494
 495     if (shouldHandleResult) {
 496         LOGD("DetectionBasedTracker::process: get _rectsWhereRegions were got from resultDetect");
 497     } else {
 498         LOGD("DetectionBasedTracker::process: get _rectsWhereRegions from previous positions");
 499         for(size_t i=0; i < trackedObjects.size(); i++) {
 500             int n=trackedObjects[i].lastPositions.size();
 501             CV_Assert(n > 0);
 502
 503             Rect r=trackedObjects[i].lastPositions[n-1];
 504             if(r.area()==0) {
 505                 LOGE("DetectionBasedTracker::process: ERROR: ATTENTION: strange algorithm's behavior: trackedObjects[i].rect() is empty");
 506                 continue;
 507             }
 508
 509
 510             //correction by speed of rectangle
 511             if (n > 1) {
 512                 Point2f center=centerRect(r);
 513                 Point2f center_prev=centerRect(trackedObjects[i].lastPositions[n-2]);
 514                 Point2f shift=(center - center_prev) * innerParameters.coeffObjectSpeedUsingInPrediction;
 515
 516                 r.x+=cvRound(shift.x);
 517                 r.y+=cvRound(shift.y);
 518             }
 519
 520
 521             rectsWhereRegions.push_back(r);
 522         }
 523     }
 524     LOGI("DetectionBasedTracker::process: tracked objects num==%d", (int)trackedObjects.size());
 525
 526     vector<Rect> detectedObjectsInRegions;
 527
 528     LOGD("DetectionBasedTracker::process: rectsWhereRegions.size()=%d", (int)rectsWhereRegions.size());
 529     for(size_t i=0; i < rectsWhereRegions.size(); i++) {
 530         Rect r=rectsWhereRegions[i];
 531
 532         detectInRegion(imageDetect, r, detectedObjectsInRegions);
 533     }
 534     LOGD("DetectionBasedTracker::process: detectedObjectsInRegions.size()=%d", (int)detectedObjectsInRegions.size());
 535
 536     updateTrackedObjects(detectedObjectsInRegions);
 537 }
 538
 539 void DetectionBasedTracker::getObjects(std::vector<cv::Rect>& result) const
 540 {
 541     result.clear();
 542
 543     for(size_t i=0; i < trackedObjects.size(); i++) {
 544         Rect r=calcTrackedObjectPositionToShow(i);
 545         if (r.area()==0) {
 546             continue;
 547         }
 548         result.push_back(r);
 549         LOGD("DetectionBasedTracker::process: found a object with SIZE %d x %d, rect={%d, %d, %d x %d}", r.width, r.height, r.x, r.y, r.width, r.height);
 550     }
 551 }
 552 void DetectionBasedTracker::getObjects(std::vector<Object>& result) const
 553 {
 554     result.clear();
 555
 556     for(size_t i=0; i < trackedObjects.size(); i++) {
 557         Rect r=calcTrackedObjectPositionToShow(i);
 558         if (r.area()==0) {
 559             continue;
 560         }
 561         result.push_back(Object(r, trackedObjects[i].id));
 562         LOGD("DetectionBasedTracker::process: found a object with SIZE %d x %d, rect={%d, %d, %d x %d}", r.width, r.height, r.x, r.y, r.width, r.height);
 563     }
 564 }
 565
 566
 567
 568 bool DetectionBasedTracker::run()
 569 {
 570     return separateDetectionWork->run();
 571 }
 572
 573 void DetectionBasedTracker::stop()
 574 {
 575     separateDetectionWork->stop();
 576 }
 577
 578 void DetectionBasedTracker::resetTracking()
 579 {
 580     separateDetectionWork->resetTracking();
 581     trackedObjects.clear();
 582 }
 583
 584 void DetectionBasedTracker::updateTrackedObjects(const vector<Rect>& detectedObjects)
 585 {
 586     enum {
 587         NEW_RECTANGLE=-1,
 588         INTERSECTED_RECTANGLE=-2
 589     };
 590
 591     int N1=trackedObjects.size();
 592     int N2=detectedObjects.size();
 593     LOGD("DetectionBasedTracker::updateTrackedObjects: N1=%d, N2=%d", N1, N2);
 594
 595     for(int i=0; i < N1; i++) {
 596         trackedObjects[i].numDetectedFrames++;
 597     }
 598
 599     vector<int> correspondence(detectedObjects.size(), NEW_RECTANGLE);
 600     correspondence.clear();
 601     correspondence.resize(detectedObjects.size(), NEW_RECTANGLE);
 602
 603     for(int i=0; i < N1; i++) {
 604         LOGD("DetectionBasedTracker::updateTrackedObjects: i=%d", i);
 605         TrackedObject& curObject=trackedObjects[i];
 606
 607         int bestIndex=-1;
 608         int bestArea=-1;
 609
 610         int numpositions=curObject.lastPositions.size();
 611         CV_Assert(numpositions > 0);
 612         Rect prevRect=curObject.lastPositions[numpositions-1];
 613         LOGD("DetectionBasedTracker::updateTrackedObjects: prevRect[%d]={%d, %d, %d x %d}", i, prevRect.x, prevRect.y, prevRect.width, prevRect.height);
 614
 615         for(int j=0; j < N2; j++) {
 616             LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d", j);
 617             if (correspondence[j] >= 0) {
 618                 LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d is rejected, because it has correspondence=%d", j, correspondence[j]);
 619                 continue;
 620             }
 621             if (correspondence[j] !=NEW_RECTANGLE) {
 622                 LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d is rejected, because it is intersected with another rectangle", j);
 623                 continue;
 624             }
 625             LOGD("DetectionBasedTracker::updateTrackedObjects: detectedObjects[%d]={%d, %d, %d x %d}",
 626                     j, detectedObjects[j].x, detectedObjects[j].y, detectedObjects[j].width, detectedObjects[j].height);
 627
 628             Rect r=prevRect & detectedObjects[j];
 629             if ( (r.width > 0) && (r.height > 0) ) {
 630                 LOGD("DetectionBasedTracker::updateTrackedObjects: There is intersection between prevRect and detectedRect, r={%d, %d, %d x %d}",
 631                         r.x, r.y, r.width, r.height);
 632                 correspondence[j]=INTERSECTED_RECTANGLE;
 633
 634                 if ( r.area() > bestArea) {
 635                     LOGD("DetectionBasedTracker::updateTrackedObjects: The area of intersection is %d, it is better than bestArea=%d", r.area(), bestArea);
 636                     bestIndex=j;
 637                     bestArea=r.area();
 638                 }
 639             }
 640         }
 641         if (bestIndex >= 0) {
 642             LOGD("DetectionBasedTracker::updateTrackedObjects: The best correspondence for i=%d is j=%d", i, bestIndex);
 643             correspondence[bestIndex]=i;
 644
 645             for(int j=0; j < N2; j++) {
 646                 if (correspondence[j] >= 0)
 647                     continue;
 648
 649                 Rect r=detectedObjects[j] & detectedObjects[bestIndex];
 650                 if ( (r.width > 0) && (r.height > 0) ) {
 651                     LOGD("DetectionBasedTracker::updateTrackedObjects: Found intersection between "
 652                             "rectangles j=%d and bestIndex=%d, rectangle j=%d is marked as intersected", j, bestIndex, j);
 653                     correspondence[j]=INTERSECTED_RECTANGLE;
 654                 }
 655             }
 656         } else {
 657             LOGD("DetectionBasedTracker::updateTrackedObjects: There is no correspondence for i=%d ", i);
 658             curObject.numFramesNotDetected++;
 659         }
 660     }
 661
 662     LOGD("DetectionBasedTracker::updateTrackedObjects: start second cycle");
 663     for(int j=0; j < N2; j++) {
 664         LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d", j);
 665         int i=correspondence[j];
 666         if (i >= 0) {//add position
 667             LOGD("DetectionBasedTracker::updateTrackedObjects: add position");
 668             trackedObjects[i].lastPositions.push_back(detectedObjects[j]);
 669             while ((int)trackedObjects[i].lastPositions.size() > (int) innerParameters.numLastPositionsToTrack) {
 670                 trackedObjects[i].lastPositions.erase(trackedObjects[i].lastPositions.begin());
 671             }
 672             trackedObjects[i].numFramesNotDetected=0;
 673         } else if (i==NEW_RECTANGLE){ //new object
 674             LOGD("DetectionBasedTracker::updateTrackedObjects: new object");
 675             trackedObjects.push_back(detectedObjects[j]);
 676         } else {
 677             LOGD("DetectionBasedTracker::updateTrackedObjects: was auxiliary intersection");
 678         }
 679     }
 680
 681     std::vector<TrackedObject>::iterator it=trackedObjects.begin();
 682     while( it != trackedObjects.end() ) {
 683         if ( (it->numFramesNotDetected > parameters.maxTrackLifetime)
 684                 ||
 685                 (
 686                  (it->numDetectedFrames <= innerParameters.numStepsToWaitBeforeFirstShow)
 687                  &&
 688                  (it->numFramesNotDetected > innerParameters.numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown)
 689                 )
 690            )
 691         {
 692             int numpos=it->lastPositions.size();
 693             CV_Assert(numpos > 0);
 694             Rect r = it->lastPositions[numpos-1];
 695             LOGD("DetectionBasedTracker::updateTrackedObjects: deleted object {%d, %d, %d x %d}",
 696                     r.x, r.y, r.width, r.height);
 697             it=trackedObjects.erase(it);
 698         } else {
 699             it++;
 700         }
 701     }
 702 }
 703 Rect DetectionBasedTracker::calcTrackedObjectPositionToShow(int i) const
 704 {
 705     if ( (i < 0) || (i >= (int)trackedObjects.size()) ) {
 706         LOGE("DetectionBasedTracker::calcTrackedObjectPositionToShow: ERROR: wrong i=%d", i);
 707         return Rect();
 708     }
 709     if (trackedObjects[i].numDetectedFrames <= innerParameters.numStepsToWaitBeforeFirstShow){
 710         LOGI("DetectionBasedTracker::calcTrackedObjectPositionToShow: trackedObjects[%d].numDetectedFrames=%d <= numStepsToWaitBeforeFirstShow=%d --- return empty Rect()",
 711                 i, trackedObjects[i].numDetectedFrames, innerParameters.numStepsToWaitBeforeFirstShow);
 712         return Rect();
 713     }
 714     if (trackedObjects[i].numFramesNotDetected > innerParameters.numStepsToShowWithoutDetecting) {
 715         return Rect();
 716     }
 717
 718     const TrackedObject::PositionsVector& lastPositions=trackedObjects[i].lastPositions;
 719
 720     int N=lastPositions.size();
 721     if (N<=0) {
 722         LOGE("DetectionBasedTracker::calcTrackedObjectPositionToShow: ERROR: no positions for i=%d", i);
 723         return Rect();
 724     }
 725
 726     int Nsize=std::min(N, (int)weightsSizesSmoothing.size());
 727     int Ncenter= std::min(N, (int)weightsPositionsSmoothing.size());
 728
 729     Point2f center;
 730     double w=0, h=0;
 731     if (Nsize > 0) {
 732         double sum=0;
 733         for(int j=0; j < Nsize; j++) {
 734             int k=N-j-1;
 735             w+= lastPositions[k].width  * weightsSizesSmoothing[j];
 736             h+= lastPositions[k].height * weightsSizesSmoothing[j];
 737             sum+=weightsSizesSmoothing[j];
 738         }
 739         w /= sum;
 740         h /= sum;
 741     } else {
 742         w=lastPositions[N-1].width;
 743         h=lastPositions[N-1].height;
 744     }
 745
 746     if (Ncenter > 0) {
 747         double sum=0;
 748         for(int j=0; j < Ncenter; j++) {
 749             int k=N-j-1;
 750             Point tl(lastPositions[k].tl());
 751             Point br(lastPositions[k].br());
 752             Point2f c1;
 753             c1=tl;
 754                 c1=c1* 0.5f;
 755             Point2f c2;
 756             c2=br;
 757             c2=c2*0.5f;
 758             c1=c1+c2;
 759
 760             center=center+  (c1  * weightsPositionsSmoothing[j]);
 761             sum+=weightsPositionsSmoothing[j];
 762         }
 763         center *= (float)(1 / sum);
 764     } else {
 765         int k=N-1;
 766         Point tl(lastPositions[k].tl());
 767         Point br(lastPositions[k].br());
 768         Point2f c1;
 769         c1=tl;
 770         c1=c1* 0.5f;
 771         Point2f c2;
 772         c2=br;
 773         c2=c2*0.5f;
 774
 775         center=c1+c2;
 776     }
 777     Point2f tl=center-(Point2f(w,h)*0.5);
 778     Rect res(cvRound(tl.x), cvRound(tl.y), cvRound(w), cvRound(h));
 779     LOGD("DetectionBasedTracker::calcTrackedObjectPositionToShow: Result for i=%d: {%d, %d, %d x %d}", i, res.x, res.y, res.width, res.height);
 780
 781     return res;
 782 }
 783
 784 void DetectionBasedTracker::detectInRegion(const Mat& img, const Rect& r, vector<Rect>& detectedObjectsInRegions)
 785 {
 786     Rect r0(Point(), img.size());
 787     Rect r1=scale_rect(r, innerParameters.coeffTrackingWindowSize);
 788     r1=r1 & r0;
 789     if ( (r1.width <=0) || (r1.height <= 0) ) {
 790         LOGD("DetectionBasedTracker::detectInRegion: Empty intersection");
 791         return;
 792     }
 793
 794     int d=std::min(r.width, r.height);
 795     d=cvRound(d * innerParameters.coeffObjectSizeToTrack);
 796
 797     vector<Rect> tmpobjects;
 798
 799     Mat img1(img, r1);//subimage for rectangle -- without data copying
 800     LOGD("DetectionBasedTracker::detectInRegion: img1.size()=%d x %d, d=%d",
 801             img1.size().width, img1.size().height, d);
 802
 803     int maxObjectSize=parameters.maxObjectSize;
 804     Size max_objectSize(maxObjectSize, maxObjectSize);
 805
 806     cascadeForTracking.detectMultiScale( img1, tmpobjects,
 807             parameters.scaleFactor, parameters.minNeighbors, 0
 808             |CV_HAAR_FIND_BIGGEST_OBJECT
 809             |CV_HAAR_SCALE_IMAGE
 810             ,
 811             Size(d,d),
 812             max_objectSize
 813             );
 814
 815     for(size_t i=0; i < tmpobjects.size(); i++) {
 816         Rect curres(tmpobjects[i].tl() + r1.tl(), tmpobjects[i].size());
 817         detectedObjectsInRegions.push_back(curres);
 818     }
 819 }
 820
 821 bool DetectionBasedTracker::setParameters(const Parameters& params)
 822 {
 823     if ( (params.minObjectSize <= 0)
 824             || (params.maxObjectSize < 0)
 825             || (params.scaleFactor <= 1.0)
 826             || (params.maxTrackLifetime < 0) )
 827     {
 828         LOGE("DetectionBasedTracker::setParameters: ERROR: wrong parameters value");
 829         return false;
 830     }
 831
 832     separateDetectionWork->lock();
 833     parameters=params;
 834     separateDetectionWork->unlock();
 835     return true;
 836 }
 837
 838 const DetectionBasedTracker::Parameters& DetectionBasedTracker::getParameters()
 839 {
 840     return parameters;
 841 }
 842
 843 #endif
 844