diff options
| -rw-r--r-- | tracker-easy/tracker-easy.cpp | 100 | ||||
| -rw-r--r-- | tracker-easy/tracker-easy.h | 8 |
2 files changed, 71 insertions, 37 deletions
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp index 971127d4..adffa33d 100644 --- a/tracker-easy/tracker-easy.cpp +++ b/tracker-easy/tracker-easy.cpp @@ -167,12 +167,10 @@ namespace EasyTracker int centerPointIndex = -1; { - QMutexLocker l(¢er_lock); + QMutexLocker l(&iProcessLock); if (success) { - ever_success.store(true, std::memory_order_relaxed); - //Bitmap origin is top left iTrackedPoints.clear(); // Tracked points must match the order of the object model points. @@ -252,7 +250,14 @@ namespace EasyTracker } } - if (movedEnough) + if (!movedEnough) + { + // We are in a dead zone + // However we still have tracking so make sure we don't auto center + QMutexLocker lock(&iDataLock); + iBestTime.start(); + } + else { // Build deadzone rectangles if needed iTrackedRects.clear(); @@ -296,7 +301,9 @@ namespace EasyTracker iTranslations.push_back(translation); } } - + + // Reset best solution index + iBestSolutionIndex = -1; if (solutionCount > 0) { @@ -331,26 +338,28 @@ namespace EasyTracker dbgout << "\n"; } - } - } - - - if (iBestSolutionIndex != -1) - { - // Best translation - cv::Vec3d translation = iTranslations[iBestSolutionIndex]; - // Best angles - cv::Vec3d angles = iAngles[iBestSolutionIndex]; - - // Pass solution through our kalman filter - iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]); - - // Send solution data back to main thread - QMutexLocker l2(&data_lock); - iBestAngles = angles; - iBestTranslation = translation; - } + if (iBestSolutionIndex != -1) + { + // Best translation + cv::Vec3d translation = iTranslations[iBestSolutionIndex]; + // Best angles + cv::Vec3d angles = iAngles[iBestSolutionIndex]; + + // Pass solution through our kalman filter + iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]); + + // We succeded in finding a solution to our PNP problem + ever_success.store(true, std::memory_order_relaxed); + + // Send solution data back to main thread + QMutexLocker l2(&iDataLock); + iBestAngles = angles; + iBestTranslation = translation; + iBestTime.start(); + } + } + } } if (doPreview) @@ -518,7 +527,7 @@ namespace EasyTracker void Tracker::UpdateDeadzones(int aHalfEdgeSize) { - QMutexLocker l(¢er_lock); + QMutexLocker l(&iProcessLock); iDeadzoneHalfEdge = aHalfEdgeSize; iDeadzoneEdge = iDeadzoneHalfEdge * 2; iTrackedRects.clear(); @@ -527,7 +536,7 @@ namespace EasyTracker void Tracker::UpdateSolver(int aSolver) { - QMutexLocker l(¢er_lock); + QMutexLocker l(&iProcessLock); iSolver = aSolver; } @@ -569,28 +578,47 @@ namespace EasyTracker } // + void FeedData(double* aData, const cv::Vec3d& aAngles, const cv::Vec3d& aTranslation) + { + aData[Yaw] = aAngles[1]; + aData[Pitch] = aAngles[0]; + aData[Roll] = aAngles[2]; + aData[TX] = aTranslation[0]; + aData[TY] = aTranslation[1]; + aData[TZ] = aTranslation[2]; + } + + // // That's called around 250 times per second. // Therefore we better not do anything here other than provide current data. // - void Tracker::data(double *data) + void Tracker::data(double* aData) { if (ever_success.load(std::memory_order_relaxed)) { // Get data back from tracker thread - QMutexLocker l(&data_lock); - data[Yaw] = iBestAngles[1]; - data[Pitch] = iBestAngles[0]; - data[Roll] = iBestAngles[2]; - data[TX] = iBestTranslation[0]; - data[TY] = iBestTranslation[1]; - data[TZ] = iBestTranslation[2]; + QMutexLocker l(&iDataLock); + // If there was no new data recently then we provide center data. + // Basically if our user remove her hat we will go back to center position until she puts it back on. + if (iBestTime.elapsed_seconds() > 1) + { + // Reset to center until we get new data + FeedData(aData, iCenterAngles, iCenterTranslation); + } + else + { + // We got valid data, provide it + FeedData(aData, iBestAngles, iBestTranslation); + } } } bool Tracker::center() { - QMutexLocker l(¢er_lock); - //TODO: Do we need to do anything there? + QMutexLocker l(&iDataLock); + iCenterTranslation = iBestTranslation; + iCenterAngles = iBestAngles; + // Returning false tells the pipeline we want to use the default center behaviour return false; } diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h index 37337087..65b777bf 100644 --- a/tracker-easy/tracker-easy.h +++ b/tracker-easy/tracker-easy.h @@ -101,7 +101,7 @@ namespace EasyTracker Preview iPreview; std::atomic<bool> ever_success = false; - mutable QMutex center_lock, data_lock; + mutable QMutex iProcessLock, iDataLock; // Deadzone int iDeadzoneEdge=0; @@ -144,6 +144,12 @@ namespace EasyTracker cv::Vec3d iBestTranslation; // Best angles cv::Vec3d iBestAngles; + // Time at which we found our last best solution + Timer iBestTime; + // Center translation + cv::Vec3d iCenterTranslation = {0,0,0}; + // Center angles + cv::Vec3d iCenterAngles = { 0,0,0 }; }; } |