detection_based_tracker.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                          License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
00014 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
00015 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00016 // Third party copyrights are property of their respective owners.
00017 //
00018 // Redistribution and use in source and binary forms, with or without modification,
00019 // are permitted provided that the following conditions are met:
00020 //
00021 //   * Redistribution's of source code must retain the above copyright notice,
00022 //     this list of conditions and the following disclaimer.
00023 //
00024 //   * Redistribution's in binary form must reproduce the above copyright notice,
00025 //     this list of conditions and the following disclaimer in the documentation
00026 //     and/or other materials provided with the distribution.
00027 //
00028 //   * The name of the copyright holders may not be used to endorse or promote products
00029 //     derived from this software without specific prior written permission.
00030 //
00031 // This software is provided by the copyright holders and contributors "as is" and
00032 // any express or implied warranties, including, but not limited to, the implied
00033 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00034 // In no event shall the Intel Corporation or contributors be liable for any direct,
00035 // indirect, incidental, special, exemplary, or consequential damages
00036 // (including, but not limited to, procurement of substitute goods or services;
00037 // loss of use, data, or profits; or business interruption) however caused
00038 // and on any theory of liability, whether in contract, strict liability,
00039 // or tort (including negligence or otherwise) arising in any way out of
00040 // the use of this software, even if advised of the possibility of such damage.
00041 //
00042 //M*/
00043 
00044 #include "precomp.hpp"
00045 
00046 #if (defined(__cplusplus) &&  __cplusplus > 199711L) || (defined(_MSC_VER) && _MSC_VER >= 1700)
00047 #define USE_STD_THREADS
00048 #endif
00049 
00050 #if defined(__linux__) || defined(LINUX) || defined(__APPLE__) || defined(ANDROID) || defined(USE_STD_THREADS)
00051 
00052 #include "opencv2/core/utility.hpp"
00053 
00054 #ifdef USE_STD_THREADS
00055 #include <thread>
00056 #include <mutex>
00057 #include <condition_variable>
00058 #else
00059 #include <pthread.h>
00060 #endif
00061 
00062 #if defined(DEBUG) || defined(_DEBUG)
00063 #undef DEBUGLOGS
00064 #define DEBUGLOGS 1
00065 #endif
00066 
00067 #ifndef DEBUGLOGS
00068 #define DEBUGLOGS 0
00069 #endif
00070 
00071 #ifdef ANDROID
00072 #include <android/log.h>
00073 #define LOG_TAG "OBJECT_DETECTOR"
00074 #define LOGD0(...) ((void)__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__))
00075 #define LOGI0(...) ((void)__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__))
00076 #define LOGW0(...) ((void)__android_log_print(ANDROID_LOG_WARN, LOG_TAG, __VA_ARGS__))
00077 #define LOGE0(...) ((void)__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__))
00078 #else
00079 
00080 #include <stdio.h>
00081 
00082 #define LOGD0(_str, ...) (printf(_str , ## __VA_ARGS__), printf("\n"), fflush(stdout))
00083 #define LOGI0(_str, ...) (printf(_str , ## __VA_ARGS__), printf("\n"), fflush(stdout))
00084 #define LOGW0(_str, ...) (printf(_str , ## __VA_ARGS__), printf("\n"), fflush(stdout))
00085 #define LOGE0(_str, ...) (printf(_str , ## __VA_ARGS__), printf("\n"), fflush(stdout))
00086 #endif
00087 
00088 #if DEBUGLOGS
00089 #define LOGD(_str, ...) LOGD0(_str , ## __VA_ARGS__)
00090 #define LOGI(_str, ...) LOGI0(_str , ## __VA_ARGS__)
00091 #define LOGW(_str, ...) LOGW0(_str , ## __VA_ARGS__)
00092 #define LOGE(_str, ...) LOGE0(_str , ## __VA_ARGS__)
00093 #else
00094 #define LOGD(...)
00095 #define LOGI(...)
00096 #define LOGW(...)
00097 #define LOGE(...)
00098 #endif
00099 
00100 
00101 using namespace cv;
00102 
00103 static inline cv::Point2f centerRect(const cv::Rect& r)
00104 {
00105     return cv::Point2f(r.x+((float)r.width)/2, r.y+((float)r.height)/2);
00106 }
00107 
00108 static inline cv::Rect scale_rect(const cv::Rect& r, float scale)
00109 {
00110     cv::Point2f m=centerRect(r);
00111     float width  = r.width  * scale;
00112     float height = r.height * scale;
00113     int x=cvRound(m.x - width/2);
00114     int y=cvRound(m.y - height/2);
00115 
00116     return cv::Rect(x, y, cvRound(width), cvRound(height));
00117 }
00118 
00119 namespace cv
00120 {
00121     void* workcycleObjectDetectorFunction(void* p);
00122 }
00123 
00124 class cv::DetectionBasedTracker::SeparateDetectionWork
00125 {
00126     public:
00127         SeparateDetectionWork(cv::DetectionBasedTracker& _detectionBasedTracker, cv::Ptr<DetectionBasedTracker::IDetector>  _detector);
00128         virtual ~SeparateDetectionWork();
00129         bool communicateWithDetectingThread(const Mat& imageGray, std::vector<Rect>& rectsWhereRegions);
00130         bool run();
00131         void stop();
00132         void resetTracking();
00133 
00134         inline bool isWorking()
00135         {
00136             return (stateThread==STATE_THREAD_WORKING_SLEEPING) || (stateThread==STATE_THREAD_WORKING_WITH_IMAGE);
00137         }
00138         inline void lock()
00139         {
00140 #ifdef USE_STD_THREADS
00141             mtx_lock.lock();
00142 #else
00143             pthread_mutex_lock(&mutex);
00144 #endif
00145         }
00146         inline void unlock()
00147         {
00148 #ifdef USE_STD_THREADS
00149             mtx_lock.unlock();
00150 #else
00151             pthread_mutex_unlock(&mutex);
00152 #endif
00153         }
00154 
00155     protected:
00156 
00157         DetectionBasedTracker& detectionBasedTracker;
00158         cv::Ptr<DetectionBasedTracker::IDetector>  cascadeInThread;
00159 #ifdef USE_STD_THREADS
00160         std::thread second_workthread;
00161         std::mutex mtx;
00162         std::unique_lock<std::mutex> mtx_lock;
00163         std::condition_variable objectDetectorRun;
00164         std::condition_variable objectDetectorThreadStartStop;
00165 #else
00166         pthread_t second_workthread;
00167         pthread_mutex_t mutex;
00168         pthread_cond_t objectDetectorRun;
00169         pthread_cond_t objectDetectorThreadStartStop;
00170 #endif
00171         std::vector<cv::Rect> resultDetect;
00172         volatile bool isObjectDetectingReady;
00173         volatile bool shouldObjectDetectingResultsBeForgot;
00174 
00175         enum StateSeparatedThread {
00176             STATE_THREAD_STOPPED=0,
00177             STATE_THREAD_WORKING_SLEEPING,
00178             STATE_THREAD_WORKING_WITH_IMAGE,
00179             STATE_THREAD_WORKING,
00180             STATE_THREAD_STOPPING
00181         };
00182         volatile StateSeparatedThread stateThread;
00183 
00184         cv::Mat imageSeparateDetecting;
00185 
00186         void workcycleObjectDetector();
00187         friend void* workcycleObjectDetectorFunction(void* p);
00188 
00189         long long  timeWhenDetectingThreadStartedWork;
00190 };
00191 
00192 cv::DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork(DetectionBasedTracker& _detectionBasedTracker, cv::Ptr<DetectionBasedTracker::IDetector>  _detector)
00193     :detectionBasedTracker(_detectionBasedTracker),
00194     cascadeInThread(),
00195     isObjectDetectingReady(false),
00196     shouldObjectDetectingResultsBeForgot(false),
00197     stateThread(STATE_THREAD_STOPPED),
00198     timeWhenDetectingThreadStartedWork(-1)
00199 {
00200     CV_Assert(_detector);
00201 
00202     cascadeInThread = _detector;
00203 #ifdef USE_STD_THREADS
00204     mtx_lock =  std::unique_lock<std::mutex>(mtx);
00205     mtx_lock.unlock();
00206 #else
00207     int res=0;
00208     res=pthread_mutex_init(&mutex, NULL);//TODO: should be attributes?
00209     if (res) {
00210         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_mutex_init(&mutex, NULL) is %d", res);
00211         throw(std::exception());
00212     }
00213     res=pthread_cond_init (&objectDetectorRun, NULL);
00214     if (res) {
00215         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_cond_init(&objectDetectorRun,, NULL) is %d", res);
00216         pthread_mutex_destroy(&mutex);
00217         throw(std::exception());
00218     }
00219     res=pthread_cond_init (&objectDetectorThreadStartStop, NULL);
00220     if (res) {
00221         LOGE("ERROR in DetectionBasedTracker::SeparateDetectionWork::SeparateDetectionWork in pthread_cond_init(&objectDetectorThreadStartStop,, NULL) is %d", res);
00222         pthread_cond_destroy(&objectDetectorRun);
00223         pthread_mutex_destroy(&mutex);
00224         throw(std::exception());
00225     }
00226 #endif
00227 }
00228 
00229 cv::DetectionBasedTracker::SeparateDetectionWork::~SeparateDetectionWork()
00230 {
00231     if(stateThread!=STATE_THREAD_STOPPED) {
00232         LOGE("\n\n\nATTENTION!!! dangerous algorithm error: destructor DetectionBasedTracker::DetectionBasedTracker::~SeparateDetectionWork is called before stopping the workthread");
00233     }
00234 #ifndef USE_STD_THREADS
00235     pthread_cond_destroy(&objectDetectorThreadStartStop);
00236     pthread_cond_destroy(&objectDetectorRun);
00237     pthread_mutex_destroy(&mutex);
00238 #endif
00239 }
00240 bool cv::DetectionBasedTracker::SeparateDetectionWork::run()
00241 {
00242     LOGD("DetectionBasedTracker::SeparateDetectionWork::run() --- start");
00243 #ifdef USE_STD_THREADS
00244     mtx_lock.lock();
00245 #else
00246     pthread_mutex_lock(&mutex);
00247 #endif
00248     if (stateThread != STATE_THREAD_STOPPED) {
00249         LOGE("DetectionBasedTracker::SeparateDetectionWork::run is called while the previous run is not stopped");
00250 #ifdef USE_STD_THREADS
00251         mtx_lock.unlock();
00252 #else
00253         pthread_mutex_unlock(&mutex);
00254 #endif
00255         return false;
00256     }
00257     stateThread=STATE_THREAD_WORKING_SLEEPING;
00258 #ifdef USE_STD_THREADS
00259     second_workthread = std::thread(workcycleObjectDetectorFunction, (void*)this); //TODO: add attributes?
00260     objectDetectorThreadStartStop.wait(mtx_lock);
00261     mtx_lock.unlock();
00262 #else
00263     pthread_create(&second_workthread, NULL, workcycleObjectDetectorFunction, (void*)this); //TODO: add attributes?
00264     pthread_cond_wait(&objectDetectorThreadStartStop, &mutex);
00265     pthread_mutex_unlock(&mutex);
00266 #endif
00267     LOGD("DetectionBasedTracker::SeparateDetectionWork::run --- end");
00268     return true;
00269 }
00270 
00271 #define CATCH_ALL_AND_LOG(_block)                                                           \
00272     try {                                                                                   \
00273         _block;                                                                             \
00274     }                                                                                       \
00275     catch(cv::Exception& e) {                                                               \
00276         LOGE0("\n %s: ERROR: OpenCV Exception caught: \n'%s'\n\n", CV_Func, e.what());      \
00277     } catch(std::exception& e) {                                                            \
00278         LOGE0("\n %s: ERROR: Exception caught: \n'%s'\n\n", CV_Func, e.what());             \
00279     } catch(...) {                                                                          \
00280         LOGE0("\n %s: ERROR: UNKNOWN Exception caught\n\n", CV_Func);                       \
00281     }
00282 
00283 void* cv::workcycleObjectDetectorFunction(void* p)
00284 {
00285     CATCH_ALL_AND_LOG({ ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->workcycleObjectDetector(); });
00286     try{
00287         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->lock();
00288         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->stateThread = cv::DetectionBasedTracker::SeparateDetectionWork::STATE_THREAD_STOPPED;
00289         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->isObjectDetectingReady=false;
00290         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->shouldObjectDetectingResultsBeForgot=false;
00291 #ifdef USE_STD_THREADS
00292         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->objectDetectorThreadStartStop.notify_one();
00293 #else
00294         pthread_cond_signal(&(((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->objectDetectorThreadStartStop));
00295 #endif
00296         ((cv::DetectionBasedTracker::SeparateDetectionWork*)p)->unlock();
00297     } catch(...) {
00298         LOGE0("DetectionBasedTracker: workcycleObjectDetectorFunction: ERROR concerning pointer, received as the function parameter");
00299     }
00300     return NULL;
00301 }
00302 
00303 void cv::DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector()
00304 {
00305     static double freq = getTickFrequency();
00306     LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- start");
00307     std::vector<Rect> objects;
00308 
00309     CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
00310 #ifdef USE_STD_THREADS
00311     mtx_lock.lock();
00312 #else
00313     pthread_mutex_lock(&mutex);
00314 #endif
00315     {
00316 #ifdef USE_STD_THREADS
00317         objectDetectorThreadStartStop.notify_one();
00318 #else
00319         pthread_cond_signal(&objectDetectorThreadStartStop);
00320 #endif
00321         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- before waiting");
00322         CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
00323 #ifdef USE_STD_THREADS
00324         objectDetectorRun.wait(mtx_lock);
00325 #else
00326         pthread_cond_wait(&objectDetectorRun, &mutex);
00327 #endif
00328         if (isWorking()) {
00329             stateThread=STATE_THREAD_WORKING_WITH_IMAGE;
00330         }
00331         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- after waiting");
00332     }
00333 #ifdef USE_STD_THREADS
00334     mtx_lock.unlock();
00335 #else
00336     pthread_mutex_unlock(&mutex);
00337 #endif
00338 
00339     bool isFirstStep=true;
00340 
00341     isObjectDetectingReady=false;
00342 
00343     while(isWorking())
00344     {
00345         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- next step");
00346 
00347         if (! isFirstStep) {
00348             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- before waiting");
00349             CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
00350 #ifdef USE_STD_THREADS
00351             mtx_lock.lock();
00352 #else
00353             pthread_mutex_lock(&mutex);
00354 #endif
00355             if (!isWorking()) {//it is a rare case, but may cause a crash
00356                 LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- go out from the workcycle from inner part of lock just before waiting");
00357 #ifdef USE_STD_THREADS
00358                 mtx_lock.unlock();
00359 #else
00360                 pthread_mutex_unlock(&mutex);
00361 #endif
00362                 break;
00363             }
00364             CV_Assert(stateThread==STATE_THREAD_WORKING_SLEEPING);
00365 #ifdef USE_STD_THREADS
00366             objectDetectorRun.wait(mtx_lock);
00367 #else
00368             pthread_cond_wait(&objectDetectorRun, &mutex);
00369 #endif
00370             if (isWorking()) {
00371                 stateThread=STATE_THREAD_WORKING_WITH_IMAGE;
00372             }
00373 #ifdef USE_STD_THREADS
00374             mtx_lock.unlock();
00375 #else
00376             pthread_mutex_unlock(&mutex);
00377 #endif
00378 
00379             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- after waiting");
00380         } else {
00381             isFirstStep=false;
00382         }
00383 
00384         if (!isWorking()) {
00385             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- go out from the workcycle just after waiting");
00386             break;
00387         }
00388 
00389 
00390         if (imageSeparateDetecting.empty()) {
00391             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- imageSeparateDetecting is empty, continue");
00392             continue;
00393         }
00394         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- start handling imageSeparateDetecting, img.size=%dx%d, img.data=0x%p",
00395                 imageSeparateDetecting.size().width, imageSeparateDetecting.size().height, (void*)imageSeparateDetecting.data);
00396 
00397 
00398         int64 t1_detect=getTickCount();
00399 
00400         cascadeInThread->detect(imageSeparateDetecting, objects);
00401 
00402         /*cascadeInThread.detectMultiScale( imageSeparateDetecting, objects,
00403                 detectionBasedTracker.parameters.scaleFactor, detectionBasedTracker.parameters.minNeighbors, 0
00404                 |CV_HAAR_SCALE_IMAGE
00405                 ,
00406                 min_objectSize,
00407                 max_objectSize
00408                 );
00409         */
00410 
00411         LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- end handling imageSeparateDetecting");
00412 
00413         if (!isWorking()) {
00414             LOGD("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- go out from the workcycle just after detecting");
00415             break;
00416         }
00417 
00418         int64 t2_detect = getTickCount();
00419         int64 dt_detect = t2_detect-t1_detect;
00420         double dt_detect_ms=((double)dt_detect)/freq * 1000.0;
00421         (void)(dt_detect_ms);
00422 
00423         LOGI("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector() --- objects num==%d, t_ms=%.4f", (int)objects.size(), dt_detect_ms);
00424 #ifdef USE_STD_THREADS
00425         mtx_lock.lock();
00426 #else
00427         pthread_mutex_lock(&mutex);
00428 #endif
00429         if (!shouldObjectDetectingResultsBeForgot) {
00430             resultDetect=objects;
00431             isObjectDetectingReady=true;
00432         } else { //shouldObjectDetectingResultsBeForgot==true
00433             resultDetect.clear();
00434             isObjectDetectingReady=false;
00435             shouldObjectDetectingResultsBeForgot=false;
00436         }
00437         if(isWorking()) {
00438             stateThread=STATE_THREAD_WORKING_SLEEPING;
00439         }
00440 #ifdef USE_STD_THREADS
00441         mtx_lock.unlock();
00442 #else
00443         pthread_mutex_unlock(&mutex);
00444 #endif
00445 
00446         objects.clear();
00447     }// while(isWorking())
00448 
00449     LOGI("DetectionBasedTracker::SeparateDetectionWork::workcycleObjectDetector: Returning");
00450 }
00451 
00452 void cv::DetectionBasedTracker::SeparateDetectionWork::stop()
00453 {
00454     //FIXME: TODO: should add quickStop functionality
00455 #ifdef USE_STD_THREADS
00456     mtx_lock.lock();
00457 #else
00458     pthread_mutex_lock(&mutex);
00459 #endif
00460     if (!isWorking()) {
00461 #ifdef USE_STD_THREADS
00462         mtx_lock.unlock();
00463 #else
00464         pthread_mutex_unlock(&mutex);
00465 #endif
00466         LOGE("SimpleHighguiDemoCore::stop is called but the SimpleHighguiDemoCore pthread is not active");
00467         return;
00468     }
00469     stateThread=STATE_THREAD_STOPPING;
00470     LOGD("DetectionBasedTracker::SeparateDetectionWork::stop: before going to sleep to wait for the signal from the workthread");
00471 #ifdef USE_STD_THREADS
00472     objectDetectorRun.notify_one();
00473     objectDetectorThreadStartStop.wait(mtx_lock);
00474     LOGD("DetectionBasedTracker::SeparateDetectionWork::stop: after receiving the signal from the workthread, stateThread=%d", (int)stateThread);
00475     mtx_lock.unlock();
00476 #else
00477     pthread_cond_signal(&objectDetectorRun);
00478     pthread_cond_wait(&objectDetectorThreadStartStop, &mutex);
00479     LOGD("DetectionBasedTracker::SeparateDetectionWork::stop: after receiving the signal from the workthread, stateThread=%d", (int)stateThread);
00480     pthread_mutex_unlock(&mutex);
00481 #endif
00482 }
00483 
00484 void cv::DetectionBasedTracker::SeparateDetectionWork::resetTracking()
00485 {
00486     LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking");
00487 #ifdef USE_STD_THREADS
00488     mtx_lock.lock();
00489 #else
00490     pthread_mutex_lock(&mutex);
00491 #endif
00492 
00493     if (stateThread == STATE_THREAD_WORKING_WITH_IMAGE) {
00494         LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking: since workthread is detecting objects at the moment, we should make cascadeInThread stop detecting and forget the detecting results");
00495         shouldObjectDetectingResultsBeForgot=true;
00496         //cascadeInThread.setStopFlag();//FIXME: TODO: this feature also should be contributed to OpenCV
00497     } else {
00498         LOGD("DetectionBasedTracker::SeparateDetectionWork::resetTracking: since workthread is NOT detecting objects at the moment, we should NOT make any additional actions");
00499     }
00500 
00501     resultDetect.clear();
00502     isObjectDetectingReady=false;
00503 
00504 #ifdef USE_STD_THREADS
00505     mtx_lock.unlock();
00506 #else
00507     pthread_mutex_unlock(&mutex);
00508 #endif
00509 
00510 }
00511 
00512 bool cv::DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread(const Mat& imageGray, std::vector<Rect>& rectsWhereRegions)
00513 {
00514     static double freq = getTickFrequency();
00515 
00516     bool shouldCommunicateWithDetectingThread = (stateThread==STATE_THREAD_WORKING_SLEEPING);
00517     LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: shouldCommunicateWithDetectingThread=%d", (shouldCommunicateWithDetectingThread?1:0));
00518 
00519     if (!shouldCommunicateWithDetectingThread) {
00520         return false;
00521     }
00522 
00523     bool shouldHandleResult = false;
00524 
00525 #ifdef USE_STD_THREADS
00526     mtx_lock.lock();
00527 #else
00528     pthread_mutex_lock(&mutex);
00529 #endif
00530 
00531     if (isObjectDetectingReady) {
00532         shouldHandleResult=true;
00533         rectsWhereRegions = resultDetect;
00534         isObjectDetectingReady=false;
00535 
00536         double lastBigDetectionDuration = 1000.0 * (((double)(getTickCount()  - timeWhenDetectingThreadStartedWork )) / freq);
00537         (void)(lastBigDetectionDuration);
00538         LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: lastBigDetectionDuration=%f ms", (double)lastBigDetectionDuration);
00539     }
00540 
00541     bool shouldSendNewDataToWorkThread = true;
00542     if (timeWhenDetectingThreadStartedWork > 0) {
00543         double time_from_previous_launch_in_ms=1000.0 * (((double)(getTickCount()  - timeWhenDetectingThreadStartedWork )) / freq); //the same formula as for lastBigDetectionDuration
00544         shouldSendNewDataToWorkThread = (time_from_previous_launch_in_ms >= detectionBasedTracker.parameters.minDetectionPeriod);
00545         LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: shouldSendNewDataToWorkThread was 1, now it is %d, since time_from_previous_launch_in_ms=%.2f, minDetectionPeriod=%d",
00546                 (shouldSendNewDataToWorkThread?1:0), time_from_previous_launch_in_ms, detectionBasedTracker.parameters.minDetectionPeriod);
00547     }
00548 
00549     if (shouldSendNewDataToWorkThread) {
00550 
00551         imageSeparateDetecting.create(imageGray.size(), CV_8UC1);
00552 
00553         imageGray.copyTo(imageSeparateDetecting);//may change imageSeparateDetecting ptr. But should not.
00554 
00555 
00556         timeWhenDetectingThreadStartedWork = getTickCount() ;
00557 
00558 #ifdef USE_STD_THREADS
00559         objectDetectorRun.notify_one();
00560 #else
00561         pthread_cond_signal(&objectDetectorRun);
00562 #endif
00563     }
00564 
00565 #ifdef USE_STD_THREADS
00566     mtx_lock.unlock();
00567 #else
00568     pthread_mutex_unlock(&mutex);
00569 #endif
00570     LOGD("DetectionBasedTracker::SeparateDetectionWork::communicateWithDetectingThread: result: shouldHandleResult=%d", (shouldHandleResult?1:0));
00571 
00572     return shouldHandleResult;
00573 }
00574 
00575 cv::DetectionBasedTracker::Parameters::Parameters()
00576 {
00577     maxTrackLifetime=5;
00578     minDetectionPeriod=0;
00579 }
00580 
00581 cv::DetectionBasedTracker::InnerParameters::InnerParameters()
00582 {
00583     numLastPositionsToTrack=4;
00584     numStepsToWaitBeforeFirstShow=6;
00585     numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown=3;
00586     numStepsToShowWithoutDetecting=3;
00587 
00588     coeffTrackingWindowSize=2.0;
00589     coeffObjectSizeToTrack=0.85f;
00590     coeffObjectSpeedUsingInPrediction=0.8f;
00591 
00592 }
00593 
00594 cv::DetectionBasedTracker::DetectionBasedTracker(cv::Ptr<IDetector>  mainDetector, cv::Ptr<IDetector>  trackingDetector, const Parameters& params)
00595     :separateDetectionWork(),
00596     parameters(params),
00597     innerParameters(),
00598     numTrackedSteps(0),
00599     cascadeForTracking(trackingDetector)
00600 {
00601     CV_Assert( (params.maxTrackLifetime >= 0)
00602 //            && mainDetector
00603             && trackingDetector );
00604 
00605     if (mainDetector) {
00606         separateDetectionWork.reset(new SeparateDetectionWork(*this, mainDetector));
00607     }
00608 
00609     weightsPositionsSmoothing.push_back(1);
00610     weightsSizesSmoothing.push_back(0.5);
00611     weightsSizesSmoothing.push_back(0.3f);
00612     weightsSizesSmoothing.push_back(0.2f);
00613 }
00614 
00615 cv::DetectionBasedTracker::~DetectionBasedTracker()
00616 {
00617 }
00618 
00619 void DetectionBasedTracker::process(const Mat& imageGray)
00620 {
00621     CV_Assert(imageGray.type()==CV_8UC1);
00622 
00623     if ( separateDetectionWork && !separateDetectionWork->isWorking() ) {
00624         separateDetectionWork->run();
00625     }
00626 
00627     static double freq = getTickFrequency();
00628     static long long time_when_last_call_started=getTickCount();
00629 
00630     {
00631         double delta_time_from_prev_call=1000.0 * (((double)(getTickCount()  - time_when_last_call_started)) / freq);
00632         (void)(delta_time_from_prev_call);
00633         LOGD("DetectionBasedTracker::process: time from the previous call is %f ms", (double)delta_time_from_prev_call);
00634         time_when_last_call_started=getTickCount();
00635     }
00636 
00637     Mat imageDetect=imageGray;
00638 
00639     std::vector<Rect> rectsWhereRegions;
00640     bool shouldHandleResult=false;
00641     if (separateDetectionWork) {
00642         shouldHandleResult = separateDetectionWork->communicateWithDetectingThread(imageGray, rectsWhereRegions);
00643     }
00644 
00645     if (shouldHandleResult) {
00646         LOGD("DetectionBasedTracker::process: get _rectsWhereRegions were got from resultDetect");
00647     } else {
00648         LOGD("DetectionBasedTracker::process: get _rectsWhereRegions from previous positions");
00649         for(size_t i = 0; i < trackedObjects.size(); i++) {
00650             size_t n = trackedObjects[i].lastPositions.size();
00651             CV_Assert(n > 0);
00652 
00653             Rect r = trackedObjects[i].lastPositions[n-1];
00654             if(r.area() == 0) {
00655                 LOGE("DetectionBasedTracker::process: ERROR: ATTENTION: strange algorithm's behavior: trackedObjects[i].rect() is empty");
00656                 continue;
00657             }
00658 
00659             //correction by speed of rectangle
00660             if (n > 1) {
00661                 Point2f  center = centerRect(r);
00662                 Point2f  center_prev = centerRect(trackedObjects[i].lastPositions[n-2]);
00663                 Point2f  shift = (center - center_prev) * innerParameters.coeffObjectSpeedUsingInPrediction;
00664 
00665                 r.x += cvRound(shift.x);
00666                 r.y += cvRound(shift.y);
00667             }
00668 
00669 
00670             rectsWhereRegions.push_back(r);
00671         }
00672     }
00673     LOGI("DetectionBasedTracker::process: tracked objects num==%d", (int)trackedObjects.size());
00674 
00675     std::vector<Rect> detectedObjectsInRegions;
00676 
00677     LOGD("DetectionBasedTracker::process: rectsWhereRegions.size()=%d", (int)rectsWhereRegions.size());
00678     for(size_t i=0; i < rectsWhereRegions.size(); i++) {
00679         Rect r = rectsWhereRegions[i];
00680 
00681         detectInRegion(imageDetect, r, detectedObjectsInRegions);
00682     }
00683     LOGD("DetectionBasedTracker::process: detectedObjectsInRegions.size()=%d", (int)detectedObjectsInRegions.size());
00684 
00685     updateTrackedObjects(detectedObjectsInRegions);
00686 }
00687 
00688 void cv::DetectionBasedTracker::getObjects(std::vector<cv::Rect>& result) const
00689 {
00690     result.clear();
00691 
00692     for(size_t i=0; i < trackedObjects.size(); i++) {
00693         Rect r=calcTrackedObjectPositionToShow((int)i);
00694         if (r.area()==0) {
00695             continue;
00696         }
00697         result.push_back(r);
00698         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);
00699     }
00700 }
00701 
00702 void cv::DetectionBasedTracker::getObjects(std::vector<Object>& result) const
00703 {
00704     result.clear();
00705 
00706     for(size_t i=0; i < trackedObjects.size(); i++) {
00707         Rect r=calcTrackedObjectPositionToShow((int)i);
00708         if (r.area()==0) {
00709             continue;
00710         }
00711         result.push_back(Object(r, trackedObjects[i].id));
00712         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);
00713     }
00714 }
00715 void cv::DetectionBasedTracker::getObjects(std::vector<ExtObject>& result) const
00716 {
00717     result.clear();
00718 
00719     for(size_t i=0; i < trackedObjects.size(); i++) {
00720         ObjectStatus status;
00721         Rect r=calcTrackedObjectPositionToShow((int)i, status);
00722         result.push_back(ExtObject(trackedObjects[i].id, r, status));
00723         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);
00724     }
00725 }
00726 
00727 bool cv::DetectionBasedTracker::run()
00728 {
00729     if (separateDetectionWork) {
00730         return separateDetectionWork->run();
00731     }
00732     return false;
00733 }
00734 
00735 void cv::DetectionBasedTracker::stop()
00736 {
00737     if (separateDetectionWork) {
00738         separateDetectionWork->stop();
00739     }
00740 }
00741 
00742 void cv::DetectionBasedTracker::resetTracking()
00743 {
00744     if (separateDetectionWork) {
00745         separateDetectionWork->resetTracking();
00746     }
00747     trackedObjects.clear();
00748 }
00749 
00750 void cv::DetectionBasedTracker::updateTrackedObjects(const std::vector<Rect>& detectedObjects)
00751 {
00752     enum {
00753         NEW_RECTANGLE=-1,
00754         INTERSECTED_RECTANGLE=-2
00755     };
00756 
00757     int N1=(int)trackedObjects.size();
00758     int N2=(int)detectedObjects.size();
00759     LOGD("DetectionBasedTracker::updateTrackedObjects: N1=%d, N2=%d", N1, N2);
00760 
00761     for(int i=0; i < N1; i++) {
00762         trackedObjects[i].numDetectedFrames++;
00763     }
00764 
00765     std::vector<int> correspondence(detectedObjects.size(), NEW_RECTANGLE);
00766     correspondence.clear();
00767     correspondence.resize(detectedObjects.size(), NEW_RECTANGLE);
00768 
00769     for(int i=0; i < N1; i++) {
00770         LOGD("DetectionBasedTracker::updateTrackedObjects: i=%d", i);
00771         TrackedObject& curObject=trackedObjects[i];
00772 
00773         int bestIndex=-1;
00774         int bestArea=-1;
00775 
00776         int numpositions=(int)curObject.lastPositions.size();
00777         CV_Assert(numpositions > 0);
00778         Rect prevRect=curObject.lastPositions[numpositions-1];
00779         LOGD("DetectionBasedTracker::updateTrackedObjects: prevRect[%d]={%d, %d, %d x %d}", i, prevRect.x, prevRect.y, prevRect.width, prevRect.height);
00780 
00781         for(int j=0; j < N2; j++) {
00782             LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d", j);
00783             if (correspondence[j] >= 0) {
00784                 LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d is rejected, because it has correspondence=%d", j, correspondence[j]);
00785                 continue;
00786             }
00787             if (correspondence[j] !=NEW_RECTANGLE) {
00788                 LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d is rejected, because it is intersected with another rectangle", j);
00789                 continue;
00790             }
00791             LOGD("DetectionBasedTracker::updateTrackedObjects: detectedObjects[%d]={%d, %d, %d x %d}",
00792                     j, detectedObjects[j].x, detectedObjects[j].y, detectedObjects[j].width, detectedObjects[j].height);
00793 
00794             Rect r=prevRect & detectedObjects[j];
00795             if ( (r.width > 0) && (r.height > 0) ) {
00796                 LOGD("DetectionBasedTracker::updateTrackedObjects: There is intersection between prevRect and detectedRect, r={%d, %d, %d x %d}",
00797                         r.x, r.y, r.width, r.height);
00798                 correspondence[j]=INTERSECTED_RECTANGLE;
00799 
00800                 if ( r.area() > bestArea) {
00801                     LOGD("DetectionBasedTracker::updateTrackedObjects: The area of intersection is %d, it is better than bestArea=%d", r.area(), bestArea);
00802                     bestIndex=j;
00803                     bestArea=r.area();
00804                 }
00805             }
00806         }
00807         if (bestIndex >= 0) {
00808             LOGD("DetectionBasedTracker::updateTrackedObjects: The best correspondence for i=%d is j=%d", i, bestIndex);
00809             correspondence[bestIndex]=i;
00810 
00811             for(int j=0; j < N2; j++) {
00812                 if (correspondence[j] >= 0)
00813                     continue;
00814 
00815                 Rect r=detectedObjects[j] & detectedObjects[bestIndex];
00816                 if ( (r.width > 0) && (r.height > 0) ) {
00817                     LOGD("DetectionBasedTracker::updateTrackedObjects: Found intersection between "
00818                             "rectangles j=%d and bestIndex=%d, rectangle j=%d is marked as intersected", j, bestIndex, j);
00819                     correspondence[j]=INTERSECTED_RECTANGLE;
00820                 }
00821             }
00822         } else {
00823             LOGD("DetectionBasedTracker::updateTrackedObjects: There is no correspondence for i=%d ", i);
00824             curObject.numFramesNotDetected++;
00825         }
00826     }
00827 
00828     LOGD("DetectionBasedTracker::updateTrackedObjects: start second cycle");
00829     for(int j=0; j < N2; j++) {
00830         LOGD("DetectionBasedTracker::updateTrackedObjects: j=%d", j);
00831         int i=correspondence[j];
00832         if (i >= 0) {//add position
00833             LOGD("DetectionBasedTracker::updateTrackedObjects: add position");
00834             trackedObjects[i].lastPositions.push_back(detectedObjects[j]);
00835             while ((int)trackedObjects[i].lastPositions.size() > (int) innerParameters.numLastPositionsToTrack) {
00836                 trackedObjects[i].lastPositions.erase(trackedObjects[i].lastPositions.begin());
00837             }
00838             trackedObjects[i].numFramesNotDetected=0;
00839         } else if (i==NEW_RECTANGLE){ //new object
00840             LOGD("DetectionBasedTracker::updateTrackedObjects: new object");
00841             trackedObjects.push_back(detectedObjects[j]);
00842         } else {
00843             LOGD("DetectionBasedTracker::updateTrackedObjects: was auxiliary intersection");
00844         }
00845     }
00846 
00847     std::vector<TrackedObject>::iterator it=trackedObjects.begin();
00848     while( it != trackedObjects.end() ) {
00849         if ( (it->numFramesNotDetected > parameters.maxTrackLifetime)
00850                 ||
00851                 (
00852                  (it->numDetectedFrames <= innerParameters.numStepsToWaitBeforeFirstShow)
00853                  &&
00854                  (it->numFramesNotDetected > innerParameters.numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown)
00855                 )
00856            )
00857         {
00858             int numpos=(int)it->lastPositions.size();
00859             CV_Assert(numpos > 0);
00860             Rect r = it->lastPositions[numpos-1];
00861             (void)(r);
00862             LOGD("DetectionBasedTracker::updateTrackedObjects: deleted object {%d, %d, %d x %d}",
00863                     r.x, r.y, r.width, r.height);
00864             it=trackedObjects.erase(it);
00865         } else {
00866             it++;
00867         }
00868     }
00869 }
00870 
00871 int cv::DetectionBasedTracker::addObject(const Rect& location)
00872 {
00873     LOGD("DetectionBasedTracker::addObject: new object {%d, %d %dx%d}",location.x, location.y, location.width, location.height);
00874     trackedObjects.push_back(TrackedObject(location));
00875     int newId = trackedObjects.back().id;
00876     LOGD("DetectionBasedTracker::addObject: newId = %d", newId);
00877     return newId;
00878 }
00879 
00880 Rect cv::DetectionBasedTracker::calcTrackedObjectPositionToShow(int i) const
00881 {
00882     ObjectStatus status;
00883     return calcTrackedObjectPositionToShow(i, status);
00884 }
00885 Rect cv::DetectionBasedTracker::calcTrackedObjectPositionToShow(int i, ObjectStatus& status) const
00886 {
00887     if ( (i < 0) || (i >= (int)trackedObjects.size()) ) {
00888         LOGE("DetectionBasedTracker::calcTrackedObjectPositionToShow: ERROR: wrong i=%d", i);
00889         status = WRONG_OBJECT;
00890         return Rect();
00891     }
00892     if (trackedObjects[i].numDetectedFrames <= innerParameters.numStepsToWaitBeforeFirstShow){
00893         LOGI("DetectionBasedTracker::calcTrackedObjectPositionToShow: trackedObjects[%d].numDetectedFrames=%d <= numStepsToWaitBeforeFirstShow=%d --- return empty Rect()",
00894                 i, trackedObjects[i].numDetectedFrames, innerParameters.numStepsToWaitBeforeFirstShow);
00895         status = DETECTED_NOT_SHOWN_YET;
00896         return Rect();
00897     }
00898     if (trackedObjects[i].numFramesNotDetected > innerParameters.numStepsToShowWithoutDetecting) {
00899         status = DETECTED_TEMPORARY_LOST;
00900         return Rect();
00901     }
00902 
00903     const TrackedObject::PositionsVector& lastPositions=trackedObjects[i].lastPositions;
00904 
00905     int N=(int)lastPositions.size();
00906     if (N<=0) {
00907         LOGE("DetectionBasedTracker::calcTrackedObjectPositionToShow: ERROR: no positions for i=%d", i);
00908         status = WRONG_OBJECT;
00909         return Rect();
00910     }
00911 
00912     int Nsize=std::min(N, (int)weightsSizesSmoothing.size());
00913     int Ncenter= std::min(N, (int)weightsPositionsSmoothing.size());
00914 
00915     Point2f  center;
00916     double w=0, h=0;
00917     if (Nsize > 0) {
00918         double sum=0;
00919         for(int j=0; j < Nsize; j++) {
00920             int k=N-j-1;
00921             w += lastPositions[k].width  * weightsSizesSmoothing[j];
00922             h += lastPositions[k].height * weightsSizesSmoothing[j];
00923             sum+=weightsSizesSmoothing[j];
00924         }
00925         w /= sum;
00926         h /= sum;
00927     } else {
00928         w=lastPositions[N-1].width;
00929         h=lastPositions[N-1].height;
00930     }
00931 
00932     if (Ncenter > 0) {
00933         double sum=0;
00934         for(int j=0; j < Ncenter; j++) {
00935             int k=N-j-1;
00936             Point  tl(lastPositions[k].tl());
00937             Point  br(lastPositions[k].br());
00938             Point2f  c1;
00939             c1=tl;
00940             c1=c1* 0.5f;
00941             Point2f  c2;
00942             c2=br;
00943             c2=c2*0.5f;
00944             c1=c1+c2;
00945 
00946             center=center+  (c1  * weightsPositionsSmoothing[j]);
00947             sum+=weightsPositionsSmoothing[j];
00948         }
00949         center *= (float)(1 / sum);
00950     } else {
00951         int k=N-1;
00952         Point  tl(lastPositions[k].tl());
00953         Point  br(lastPositions[k].br());
00954         Point2f  c1;
00955         c1=tl;
00956         c1=c1* 0.5f;
00957         Point2f  c2;
00958         c2=br;
00959         c2=c2*0.5f;
00960 
00961         center=c1+c2;
00962     }
00963     Point2f  tl=center-Point2f ((float)w*0.5f,(float)h*0.5f);
00964     Rect res(cvRound(tl.x), cvRound(tl.y), cvRound(w), cvRound(h));
00965     LOGD("DetectionBasedTracker::calcTrackedObjectPositionToShow: Result for i=%d: {%d, %d, %d x %d}", i, res.x, res.y, res.width, res.height);
00966 
00967     status = DETECTED;
00968     return res;
00969 }
00970 
00971 void cv::DetectionBasedTracker::detectInRegion(const Mat& img, const Rect& r, std::vector<Rect>& detectedObjectsInRegions)
00972 {
00973     Rect r0(Point (), img.size());
00974     Rect r1 = scale_rect(r, innerParameters.coeffTrackingWindowSize);
00975     r1 = r1 & r0;
00976 
00977     if ( (r1.width <=0) || (r1.height <= 0) ) {
00978         LOGD("DetectionBasedTracker::detectInRegion: Empty intersection");
00979         return;
00980     }
00981 
00982     int d = cvRound(std::min(r.width, r.height) * innerParameters.coeffObjectSizeToTrack);
00983 
00984     std::vector<Rect> tmpobjects;
00985 
00986     Mat img1(img, r1);//subimage for rectangle -- without data copying
00987     LOGD("DetectionBasedTracker::detectInRegion: img1.size()=%d x %d, d=%d",
00988             img1.size().width, img1.size().height, d);
00989 
00990     cascadeForTracking->setMinObjectSize(Size(d, d));
00991     cascadeForTracking->detect(img1, tmpobjects);
00992             /*
00993             detectMultiScale( img1, tmpobjects,
00994             parameters.scaleFactor, parameters.minNeighbors, 0
00995             |CV_HAAR_FIND_BIGGEST_OBJECT
00996             |CV_HAAR_SCALE_IMAGE
00997             ,
00998             Size(d,d),
00999             max_objectSize
01000             );*/
01001 
01002     for(size_t i=0; i < tmpobjects.size(); i++) {
01003         Rect curres(tmpobjects[i].tl() + r1.tl(), tmpobjects[i].size());
01004         detectedObjectsInRegions.push_back(curres);
01005     }
01006 }
01007 
01008 bool cv::DetectionBasedTracker::setParameters(const Parameters& params)
01009 {
01010     if ( params.maxTrackLifetime < 0 )
01011     {
01012         LOGE("DetectionBasedTracker::setParameters: ERROR: wrong parameters value");
01013         return false;
01014     }
01015 
01016     if (separateDetectionWork) {
01017         separateDetectionWork->lock();
01018     }
01019     parameters=params;
01020     if (separateDetectionWork) {
01021         separateDetectionWork->unlock();
01022     }
01023     return true;
01024 }
01025 
01026 const cv::DetectionBasedTracker::Parameters& DetectionBasedTracker::getParameters() const
01027 {
01028     return parameters;
01029 }
01030 
01031 #endif
01032