Easy Tracker: Deadzone implementation to minimize noise.

Added Deadzone pixels size to settings.
Clip and Custom model settings tabs disabled.

1 files changed, 83 insertions, 32 deletions

diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index e0ab5f7c..199bf95a 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -48,7 +48,12 @@ namespace EasyTracker
         // We could not get this working, nevermind
         //connect(&iSettings.cam_fps, value_::value_changed<int>(), this, &Tracker::SetFps, Qt::DirectConnection);
 
+        // Make sure deadzones are updated whenever the settings are changed
+        connect(&iSettings.DeadzoneRectHalfEdgeSize, value_::value_changed<int>(), this, &Tracker::UpdateDeadzones, Qt::DirectConnection);
+
         CreateModelFromSettings();
+
+        UpdateDeadzones(iSettings.DeadzoneRectHalfEdgeSize);
     }
 
     Tracker::~Tracker()
@@ -198,47 +203,79 @@ namespace EasyTracker
                 iTrackedPoints.push_back(iPoints[leftPointIndex]);
                 iTrackedPoints.push_back(iPoints[topPointIndex]);
 
-                dbgout << "Object: " << iModel << "\n";
-                dbgout << "Points: " << iTrackedPoints << "\n";
+                bool movedEnough = true;
+                // Check if we moved enough since last time we were here
+                // This is our deadzone management
+                if (iSettings.DeadzoneRectHalfEdgeSize != 0 // Check if deazones are enabled
+                    && iTrackedRects.size() == iTrackedPoints.size())
+                {
+                    movedEnough = false;
+                    for (size_t i = 0; i < iTrackedPoints.size(); i++)
+                    {
+                        if (!iTrackedRects[i].contains(iTrackedPoints[i]))
+                        {
+                            movedEnough = true;
+                            break;
+                        }
+                    }
+                }
+
+                if (movedEnough)
+                {
+                    // Build deadzone rectangles if needed
+                    iTrackedRects.clear();
+                    if (iSettings.DeadzoneRectHalfEdgeSize != 0) // Check if deazones are enabled
+                    {
+                        for (const cv::Point& pt : iTrackedPoints)
+                        {
+                            cv::Rect rect(pt - cv::Point(iDeadzoneHalfEdge, iDeadzoneHalfEdge), cv::Size(iDeadzoneEdge, iDeadzoneEdge));
+                            iTrackedRects.push_back(rect);
+                        }
+                    }
+
 
+                    dbgout << "Object: " << iModel << "\n";
+                    dbgout << "Points: " << iTrackedPoints << "\n";
 
-                // TODO: try SOLVEPNP_AP3P too, make it a settings option?
-                iAngles.clear();
-                iBestSolutionIndex = -1;
-                int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, cv::SOLVEPNP_P3P);
 
-                if (solutionCount > 0)
-                {
-                    dbgout << "Solution count: " << solutionCount << "\n";
-                    int minPitch = std::numeric_limits<int>::max();
-                    // Find the solution we want amongst all possible ones
-                    for (int i = 0; i < solutionCount; i++)
+                    // TODO: try SOLVEPNP_AP3P too, make it a settings option?
+                    iAngles.clear();
+                    iBestSolutionIndex = -1;
+                    int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, cv::SOLVEPNP_P3P);
+
+                    if (solutionCount > 0)
                     {
-                        dbgout << "Translation:\n";
-                        dbgout << iTranslations.at(i);
-                        dbgout << "\n";
-                        dbgout << "Rotation:\n";
-                        //dbgout << rvecs.at(i);
-                        cv::Mat rotationCameraMatrix;
-                        cv::Rodrigues(iRotations[i], rotationCameraMatrix);
-                        cv::Vec3d angles;
-                        getEulerAngles(rotationCameraMatrix, angles);
-                        iAngles.push_back(angles);
-
-                        // Check if pitch is closest to zero
-                        int absolutePitch = std::abs(angles[0]);
-                        if (minPitch > absolutePitch)
+                        dbgout << "Solution count: " << solutionCount << "\n";
+                        int minPitch = std::numeric_limits<int>::max();
+                        // Find the solution we want amongst all possible ones
+                        for (int i = 0; i < solutionCount; i++)
                         {
-                            // The solution with pitch closest to zero is the one we want
-                            minPitch = absolutePitch;
-                            iBestSolutionIndex = i;
+                            dbgout << "Translation:\n";
+                            dbgout << iTranslations.at(i);
+                            dbgout << "\n";
+                            dbgout << "Rotation:\n";
+                            //dbgout << rvecs.at(i);
+                            cv::Mat rotationCameraMatrix;
+                            cv::Rodrigues(iRotations[i], rotationCameraMatrix);
+                            cv::Vec3d angles;
+                            getEulerAngles(rotationCameraMatrix, angles);
+                            iAngles.push_back(angles);
+
+                            // Check if pitch is closest to zero
+                            int absolutePitch = std::abs(angles[0]);
+                            if (minPitch > absolutePitch)
+                            {
+                                // The solution with pitch closest to zero is the one we want
+                                minPitch = absolutePitch;
+                                iBestSolutionIndex = i;
+                            }
+
+                            dbgout << angles;
+                            dbgout << "\n";
                         }
 
-                        dbgout << angles;
                         dbgout << "\n";
                     }
-
-                    dbgout << "\n";
                 }
             }
 
@@ -274,6 +311,12 @@ namespace EasyTracker
                 iPreview.DrawCross(iPoints[topPointIndex]);
             }
 
+            // Render our deadzone rects
+            for (const cv::Rect& rect : iTrackedRects)
+            {
+                cv::rectangle(iPreview.iFrameRgb,rect,cv::Scalar(255,0,0));
+            }
+
             // Show full size preview pop-up
             if (iSettings.debug)
             {
@@ -397,6 +440,14 @@ namespace EasyTracker
         iKf.Init(18, 6, 0, dt);
     }
 
+    void Tracker::UpdateDeadzones(int aHalfEdgeSize)
+    {
+        QMutexLocker l(&center_lock);
+        iDeadzoneHalfEdge = aHalfEdgeSize;
+        iDeadzoneEdge = iDeadzoneHalfEdge * 2;
+        iTrackedRects.clear();
+    }
+
 
     module_status Tracker::start_tracker(QFrame* video_frame)
     {