From 389f77790d649a9da852ff7df3e0717b4c4dc1c3 Mon Sep 17 00:00:00 2001 From: Stéphane Lenclud Date: Sun, 28 Apr 2019 09:14:13 +0200 Subject: Easy Tracker: Adding basic bad pose filter based on pitch consistency. Working toward five vertices support. --- tracker-easy/tracker-easy.cpp | 203 ++++++++++++++++++++++++++---------------- tracker-easy/tracker-easy.h | 8 +- 2 files changed, 132 insertions(+), 79 deletions(-) diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp index 68729567..220765cc 100644 --- a/tracker-easy/tracker-easy.cpp +++ b/tracker-easy/tracker-easy.cpp @@ -24,11 +24,14 @@ using namespace options; // Disable debug -#define dbgout if (true) {} else std::cout +#define dbgout if (true) {} else std::cout << "\n" <::max(); + for (int i = 0; i < iPoints.size(); i++) + { + if (iPoints[i].y < minY) + { + minY = iPoints[i].y; + aTopIndex = i; + } + } + + + int maxX = 0; + + // Find right most point + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point + if (i != aTopIndex && iPoints[i].x > maxX) + { + maxX = iPoints[i].x; + aRightIndex = i; + } + } + + // Find left most point + int minX = std::numeric_limits::max(); + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point and right most point + if (i != aTopIndex && i != aRightIndex && iPoints[i].x < minX) + { + aLeftIndex = i; + minX = iPoints[i].x; + } + } + + // Find center point, the last one + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding the three points we already have + if (i != aTopIndex && i != aRightIndex && i != aLeftIndex) + { + aCenterIndex = i; + } + } + + // Order matters + iTrackedPoints.push_back(iPoints[aTopIndex]); + iTrackedPoints.push_back(iPoints[aRightIndex]); + iTrackedPoints.push_back(iPoints[aLeftIndex]); + if (iModel.size() > iTrackedPoints.size()) + { + // We are tracking more than 3 points + iTrackedPoints.push_back(iPoints[aCenterIndex]); + } + + + } + + + /// /// /// @@ -199,71 +293,13 @@ namespace EasyTracker int rightPointIndex = -1; int leftPointIndex = -1; int centerPointIndex = -1; - - + int topRightPointIndex = -1; + int topLeftPointIndex = -1; if (success) { - //Bitmap origin is top left - iTrackedPoints.clear(); - // Tracked points must match the order of the object model points. - // Find top most point, that's the one with min Y as we assume our guy's head is not up side down - int minY = std::numeric_limits::max(); - for (int i = 0; i < iPoints.size(); i++) - { - if (iPoints[i].y < minY) - { - minY = iPoints[i].y; - topPointIndex = i; - } - } - - - int maxX = 0; - - // Find right most point - for (int i = 0; i < iPoints.size(); i++) - { - // Excluding top most point - if (i != topPointIndex && iPoints[i].x > maxX) - { - maxX = iPoints[i].x; - rightPointIndex = i; - } - } - - // Find left most point - int minX = std::numeric_limits::max(); - for (int i = 0; i < iPoints.size(); i++) - { - // Excluding top most point and right most point - if (i != topPointIndex && i != rightPointIndex && iPoints[i].x < minX) - { - leftPointIndex = i; - minX = iPoints[i].x; - } - } - - // Find center point, the last one - for (int i = 0; i < iPoints.size(); i++) - { - // Excluding the three points we already have - if (i != topPointIndex && i != rightPointIndex && i != leftPointIndex) - { - centerPointIndex = i; - } - } - - // Order matters - iTrackedPoints.push_back(iPoints[topPointIndex]); - iTrackedPoints.push_back(iPoints[rightPointIndex]); - iTrackedPoints.push_back(iPoints[leftPointIndex]); - if (iModel.size() > iTrackedPoints.size()) - { - // We are tracking more than 3 points - iTrackedPoints.push_back(iPoints[centerPointIndex]); - } - + // Lets match our 3D vertices with our image 2D points + MatchVertices(topPointIndex, rightPointIndex, leftPointIndex, centerPointIndex, topRightPointIndex, topLeftPointIndex); bool movedEnough = true; // Check if we moved enough since last time we were here @@ -381,22 +417,35 @@ namespace EasyTracker // 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); + // Check if our solution makes sense + // For now, just discard solutions with extrem pitch + if (std::abs(angles[0]) > 50) //TODO: Put that in settings + { + infout << "WARNING: discarding solution!"; + iBadSolutionCount++; + } + else + { + iGoodSolutionCount++; + // 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(); + } - // Send solution data back to main thread - QMutexLocker l2(&iDataLock); - iBestAngles = angles; - iBestTranslation = translation; - iBestTime.start(); } } } if (doPreview) { + double qualityIndex = 1 - (iGoodSolutionCount!=0?(double)iBadSolutionCount / (double)iGoodSolutionCount:0); std::ostringstream ss; - ss << "FPS: " << iFps << "/" << iSkippedFps; + ss << "FPS: " << iFps << "/" << iSkippedFps << " QI: " << qualityIndex; iPreview.DrawInfo(ss.str()); //Color is BGR @@ -425,8 +474,6 @@ namespace EasyTracker iPreview.DrawCross(iPoints[centerPointIndex], color); } - - // Render our deadzone rects for (const cv::Rect& rect : iTrackedRects) { @@ -561,7 +608,7 @@ namespace EasyTracker /// void Tracker::UpdateSettings() { - infout << std::chrono::system_clock::now().time_since_epoch().count() << ": Update Setting\n"; + infout << "Update Setting"; QMutexLocker l(&iProcessLock); iPointExtractor.UpdateSettings(); iSolver = iSettings.PnpSolver; diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h index f36ea598..e1dcee55 100644 --- a/tracker-easy/tracker-easy.h +++ b/tracker-easy/tracker-easy.h @@ -68,7 +68,11 @@ namespace EasyTracker private: void UpdateModel(); void CreateCameraIntrinsicsMatrices(); - void ProcessFrame(); + void ProcessFrame(); + void MatchVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex, int& aTopRight, int& aTopLeft); + void MatchThreeOrFourVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex); + void MatchFiveVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aTopRight, int& aTopLeft); + // @@ -118,6 +122,8 @@ namespace EasyTracker int iSkippedFrameCount = 0; int iFps = 0; int iSkippedFps = 0; + uint iBadSolutionCount = 0; + uint iGoodSolutionCount = 0; // KalmanFilterPose iKf; -- cgit v1.2.3