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