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