4 files changed, 50 insertions, 4 deletions
diff --git a/tracker-easy/preview.cpp b/tracker-easy/preview.cpp
index 364e45ca..4d6b7c61 100644
--- a/tracker-easy/preview.cpp
+++ b/tracker-easy/preview.cpp
@@ -100,6 +100,11 @@ namespace EasyTracker
             QImage::Format_ARGB32);
     }
 
+    void Preview::DrawInfo(const std::string& aString)
+    {
+        cv::putText(iFrameRgb, aString, cv::Point(4,iFrameRgb.size().height-4), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0),2);
+    }
+
     void Preview::DrawCross(cv::Point aPoint)
     {
         constexpr int len = 9;
diff --git a/tracker-easy/preview.h b/tracker-easy/preview.h
index 979d272b..74ef89aa 100644
--- a/tracker-easy/preview.h
+++ b/tracker-easy/preview.h
@@ -22,10 +22,11 @@ namespace EasyTracker
         Preview& operator=(const cv::Mat& frame);
         QImage get_bitmap();
         void DrawCross(cv::Point aPoint);
+        void DrawInfo(const std::string& aString);
 
         operator cv::Mat&() { return iFrameResized; }
         operator cv::Mat const&() const { return iFrameResized; }
-
+       
     private:
         static void ensure_size(cv::Mat& frame, int w, int h, int type);
 
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index 8bae883a..68e3be36 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -27,8 +27,10 @@ using namespace options;
 
 // Disable debug
 #define dbgout if (true) {} else std::cout
+//#define infout if (true) {} else std::cout
 // Enable debug
 //#define dbgout if (false) {} else std::cout
+#define infout if (false) {} else std::cout
 
 namespace EasyTracker
 {
@@ -125,10 +127,13 @@ namespace EasyTracker
     {
         maybe_reopen_camera();
 
+        iFpsTimer.start();
+
         while (!isInterruptionRequested())
         {
-            bool new_frame = false;
+            iTimer.start();
 
+            bool new_frame = false;
             {
                 QMutexLocker l(&camera_mtx);
 
@@ -144,7 +149,7 @@ namespace EasyTracker
                 // Create OpenCV matrix from our frame
                 // TODO: Assert channel size is one or two
                 iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE((iFrame.channelSize == 2 ? CV_16U : CV_8U), iFrame.channels), iFrame.data, iFrame.stride);
-
+                iFrameCount++;
 
                 const bool preview_visible = check_is_visible();
                 if (preview_visible)
@@ -268,7 +273,11 @@ namespace EasyTracker
                 }
 
                 if (preview_visible)
-                {                    
+                {
+                    std::ostringstream ss;
+                    ss << "FPS: " << iFps << "/" << iSkippedFps;
+                    iPreview.DrawInfo(ss.str());
+
                     //
                     if (topPointIndex != -1)
                     {
@@ -302,6 +311,27 @@ namespace EasyTracker
                         cv::destroyWindow("Preview");
                     }                    
                 }
+
+                dbgout << "Frame time:" << iTimer.elapsed_seconds() << "\n";
+            }
+            else
+            {
+                iSkippedFrameCount++;
+            }
+
+            // Pace ourselves, drastically reduce CPU usage
+            // TODO: Consider using QTimer instead of QThread
+            msleep(1000 / 55);
+
+            // Compute FPS
+            double elapsed = iFpsTimer.elapsed_seconds();
+            if (elapsed >= 1.0)
+            {
+                iFps = iFrameCount / elapsed;
+                iSkippedFps = iSkippedFrameCount / elapsed;
+                iFrameCount = 0;
+                iSkippedFrameCount = 0;
+                iFpsTimer.start();
             }
         }
     }
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index 00e24811..257dd6ca 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -12,6 +12,8 @@
 #include "cv/numeric.hpp"
 #include "video/video-widget.hpp"
 #include "video/camera.hpp"
+#include "compat/timer.hpp"
+
 #include "preview.h"
 #include "settings.h"
 #include "point-extractor.h"
@@ -78,6 +80,14 @@ namespace EasyTracker
         std::atomic<bool> ever_success = false;
         mutable QMutex center_lock, data_lock;
 
+        //
+        Timer iTimer;
+        Timer iFpsTimer;
+        int iFrameCount = 0;
+        int iSkippedFrameCount = 0;
+        int iFps = 0;
+        int iSkippedFps = 0;
+
         // Vertices defining the model we are tracking
         std::vector<cv::Point3f> iModel;
         // Bitmap points corresponding to model vertices