the do
Opencv 3.1 project on GR-PEACH board
Fork of
gr-peach-opencv-project
by 
the do
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 15:17:22 by
1.7.2