EasyTracker: Now using a timer.

4 files changed, 89 insertions, 59 deletions

diff --git a/tracker-easy/kalman-filter-pose.cpp b/tracker-easy/kalman-filter-pose.cpp
index 0f6bdbdb..42346bb5 100644
--- a/tracker-easy/kalman-filter-pose.cpp
+++ b/tracker-easy/kalman-filter-pose.cpp
@@ -81,6 +81,7 @@ namespace EasyTracker
         //  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
         //  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
 
+        // Those will scale the filtered values, 2 will give you half the raw input
         measurementMatrix.at<double>(0, 0) = 1;  // x
         measurementMatrix.at<double>(1, 1) = 1;  // y
         measurementMatrix.at<double>(2, 2) = 1;  // z
diff --git a/tracker-easy/settings.h b/tracker-easy/settings.h
index ed5d0abd..dcfb6be0 100644
--- a/tracker-easy/settings.h
+++ b/tracker-easy/settings.h
@@ -48,8 +48,8 @@ namespace EasyTracker {
             clip_bz{ b, "clip-bz", 80 };
 
         value<int> active_model_panel{ b, "active-model-panel", 0 },
-            cap_x{ b, "cap-x", 40 },
-            cap_y{ b, "cap-y", 60 },
+            cap_x{ b, "cap-x", 35 },
+            cap_y{ b, "cap-y", 55 },
             cap_z{ b, "cap-z", 100 };
 
         value<int> fov{ b, "camera-fov", 56 };
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index e432b5da..e0ab5f7c 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -45,25 +45,18 @@ namespace EasyTracker
 
         connect(&iSettings.fov, value_::value_changed<int>(), this, &Tracker::set_fov, Qt::DirectConnection);
         set_fov(iSettings.fov);
+        // We could not get this working, nevermind
+        //connect(&iSettings.cam_fps, value_::value_changed<int>(), this, &Tracker::SetFps, Qt::DirectConnection);
 
         CreateModelFromSettings();
-
-        
-        //int nStates = 18;            // the number of states
-        //int nMeasurements = 6;       // the number of measured states
-        //int nInputs = 0;             // the number of control actions
-        //double dt = 0.125;           // time between measurements (1/FPS)
-
-        iKf.Init(18,6,0,0033);
     }
 
     Tracker::~Tracker()
     {
-        //
         cv::destroyWindow("Preview");
 
-        requestInterruption();
-        wait();
+        iThread.exit();
+        iThread.wait();        
 
         QMutexLocker l(&camera_mtx);
         camera->stop();        
@@ -315,51 +308,43 @@ namespace EasyTracker
     ///
     ///
     ///
-    void Tracker::run()
+    void Tracker::Tick()
     {
         maybe_reopen_camera();
+      
+        iTimer.start();
 
-        iFpsTimer.start();
-
-        while (!isInterruptionRequested())
+        bool new_frame = false;
         {
-            iTimer.start();
-
-            bool new_frame = false;
-            {
-                QMutexLocker l(&camera_mtx);
-
-                if (camera)
-                {
-                    std::tie(iFrame, new_frame) = camera->get_frame();
-                }
-                    
-            }
+            QMutexLocker l(&camera_mtx);
 
-            if (new_frame)
-            {
-                ProcessFrame();
-            }
-            else
+            if (camera)
             {
-                iSkippedFrameCount++;
+                std::tie(iFrame, new_frame) = camera->get_frame();
             }
+                    
+        }
 
-            // Pace ourselves, drastically reduce CPU usage
-            // TODO: Consider using QTimer instead of QThread
-            msleep(1000 / 55);
+        if (new_frame)
+        {
+            ProcessFrame();
+        }
+        else
+        {
+            iSkippedFrameCount++;
+        }
 
-            // Compute FPS
-            double elapsed = iFpsTimer.elapsed_seconds();
-            if (elapsed >= 1.0)
-            {
-                iFps = iFrameCount / elapsed;
-                iSkippedFps = iSkippedFrameCount / elapsed;
-                iFrameCount = 0;
-                iSkippedFrameCount = 0;
-                iFpsTimer.start();
-            }
+        // Compute FPS
+        double elapsed = iFpsTimer.elapsed_seconds();
+        if (elapsed >= 1.0)
+        {
+            iFps = iFrameCount / elapsed;
+            iSkippedFps = iSkippedFrameCount / elapsed;
+            iFrameCount = 0;
+            iSkippedFrameCount = 0;
+            iFpsTimer.start();
         }
+        
     }
 
     bool Tracker::maybe_reopen_camera()
@@ -378,6 +363,10 @@ namespace EasyTracker
         bool res = camera->start(iCameraInfo);
         // We got new our camera intrinsics, create corresponding matrices
         CreateCameraIntrinsicsMatrices();
+
+        // If ever the camera implementation provided an FPS now is the time to apply it
+        DoSetFps(iCameraInfo.fps);
+
         return res;
     }
 
@@ -387,6 +376,28 @@ namespace EasyTracker
 
     }
 
+    // Calling this from another thread than the one it belongs too after it's started somehow breaks our timer
+    void Tracker::SetFps(int aFps)
+    {
+        QMutexLocker l(&camera_mtx);
+        DoSetFps(aFps);
+    }
+
+    void Tracker::DoSetFps(int aFps)
+    {
+        // Aplly FPS to timer
+        iTicker.setInterval(1000 / aFps + 1);
+
+        // Reset Kalman filter
+        //int nStates = 18;            // the number of states
+        //int nMeasurements = 6;       // the number of measured states
+        //int nInputs = 0;             // the number of control actions
+        //double dt = 0.125;           // time between measurements (1/FPS)
+        double dt = 1000.0 / aFps;
+        iKf.Init(18, 6, 0, dt);
+    }
+
+
     module_status Tracker::start_tracker(QFrame* video_frame)
     {
         //video_frame->setAttribute(Qt::WA_NativeWindow);
@@ -401,12 +412,26 @@ namespace EasyTracker
 
         // Create our camera
         camera = video::make_camera(iSettings.camera_name);
-
-        start(QThread::HighPriority);
+        // Precise timer is needed otherwise the interval is not really respected
+        iTicker.setTimerType(Qt::PreciseTimer);
+        SetFps(iSettings.cam_fps);
+        iTicker.moveToThread(&iThread);
+        // Connect timer timeout signal to our tick slot
+        connect(&iTicker, SIGNAL(timeout()), SLOT(Tick()), Qt::DirectConnection);
+        // Start our timer once our thread is started
+        iTicker.connect(&iThread, SIGNAL(started()), SLOT(start()));
+        iFpsTimer.start(); // Kick off our FPS counter
+        iThread.setObjectName("EasyTrackerThread");
+        iThread.setPriority(QThread::HighPriority); // Do we really want that?
+        iThread.start();
 
         return {};
     }
 
+    //
+    // 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)
     {
         if (ever_success.load(std::memory_order_relaxed))
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index a84d725e..05fdb94a 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -39,11 +39,9 @@ namespace EasyTracker
 
     using namespace numeric_types;
 
-    struct Tracker : QThread, ITracker
+    struct Tracker : public QObject, ITracker
     {
-        // We had problem where Qt slots would not get disconnected upon object destruction.
-        // Issue seems to be gone now even without Q_OBJECT declaration, go figure.
-        //Q_OBJECT
+        Q_OBJECT
     public:        
         friend class Dialog;
 
@@ -54,20 +52,26 @@ namespace EasyTracker
         module_status start_tracker(QFrame* parent_window) override;
         void data(double* data) override;
         bool center() override;
-       
+
+    private slots:
+        void Tick();
+
+
     private:
         void CreateModelFromSettings();
         void CreateCameraIntrinsicsMatrices();
         void ProcessFrame();
 
-        // From QThread
-        void run() override;
+        //
 
         bool maybe_reopen_camera();
         void set_fov(int value);
+        void SetFps(int aFps);
+        void DoSetFps(int aFps);
 
         QMutex camera_mtx;
-
+        QThread iThread;
+        QTimer iTicker;
 
         Settings iSettings;
 
@@ -91,7 +95,7 @@ namespace EasyTracker
 
         // Statistics
         Timer iTimer;
-        Timer iFpsTimer;
+        Timer iFpsTimer;        
         int iFrameCount = 0;
         int iSkippedFrameCount = 0;
         int iFps = 0;