tracker/pt: fix camera Hz always the default value

3 files changed, 58 insertions, 99 deletions

diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp
index 435348b1..60eb4bb8 100644
--- a/tracker-pt/camera.cpp
+++ b/tracker-pt/camera.cpp
@@ -6,32 +6,9 @@
  */
 
 #include "camera.h"
-#include "compat/camera-names.hpp"
 #include <string>
 #include <QDebug>
 
-Camera::~Camera()
-{
-    stop();
-}
-
-void Camera::set_device(const QString& name)
-{
-    const int index = camera_name_to_index(name);
-
-    desired_name = name;
-
-    if (index != cam_desired.idx || desired_name != active_name)
-    {
-        stop();
-        cam_desired.idx = index;
-
-        // reset fps
-        dt_valid = 0;
-        dt_mean = 0;
-    }
-}
-
 QString Camera::get_desired_name() const
 {
     return desired_name;
@@ -42,30 +19,6 @@ QString Camera::get_active_name() const
     return active_name;
 }
 
-void Camera::set_fps(int fps)
-{
-    if (cam_desired.fps != fps)
-    {
-        cam_desired.fps = fps;
-        if (cap)
-            cap->set(cv::CAP_PROP_FPS, cam_desired.fps);
-    }
-}
-
-void Camera::set_res(int x_res, int y_res)
-{
-    if (cam_desired.res_x != x_res || cam_desired.res_y != y_res)
-    {
-        cam_desired.res_x = x_res;
-        cam_desired.res_y = y_res;
-        if (cap)
-        {
-            cap->set(cv::CAP_PROP_FRAME_WIDTH,  cam_desired.res_x);
-            cap->set(cv::CAP_PROP_FRAME_HEIGHT, cam_desired.res_y);
-        }
-    }
-}
-
 DEFUN_WARN_UNUSED bool Camera::get_info(CamInfo& ret)
 {
     if (cam_info.res_x == 0 || cam_info.res_y == 0)
@@ -78,7 +31,7 @@ bool Camera::get_frame(double dt, cv::Mat* frame)
 {
     bool new_frame = _get_frame(frame);
     // measure fps of valid frames
-    static constexpr double RC = 1; // seconds
+    static constexpr double RC = .1; // seconds
     const double alpha = dt/(dt + RC);
     dt_valid += dt;
     if (new_frame)
@@ -86,8 +39,8 @@ bool Camera::get_frame(double dt, cv::Mat* frame)
         if (dt_mean < 2e-3)
             dt_mean = dt;
         else
-            dt_mean = alpha * dt_mean + (1 - alpha) * dt_valid;
-        cam_info.fps = int(std::round(dt_mean > 2e-3 ? 1 / dt_mean : 0));
+            dt_mean = (1-alpha) * dt_mean + alpha * dt_valid;
+        cam_info.fps = dt_mean > 2e-3 ? int(1 / dt_mean + .65) : 0;
         dt_valid = 0;
     }
     else
@@ -95,22 +48,40 @@ bool Camera::get_frame(double dt, cv::Mat* frame)
     return new_frame;
 }
 
-void Camera::start()
+DEFUN_WARN_UNUSED bool Camera::start(int idx, int fps, int res_x, int res_y)
 {
-    cap = camera_ptr(new cv::VideoCapture(cam_desired.idx));
+    if (idx >= 0 && fps >= 0 && res_x > 0 && res_y > 0)
+    {
+        if (!cap || cap->isOpened() ||
+            cam_desired.idx != idx ||
+            cam_desired.fps != fps ||
+            cam_desired.res_x != res_x ||
+            cam_desired.res_y != res_y)
+        {
+            cam_desired.idx = idx;
+            cam_desired.fps = fps;
+            cam_desired.res_x = res_x;
+            cam_desired.res_y = res_y;
 
-    set_res(cam_desired.res_x, cam_desired.res_y);
-    set_fps(cam_desired.fps);
+            cap = camera_ptr(new cv::VideoCapture(cam_desired.idx));
 
-    if (cap->isOpened())
-    {
-        cam_info.idx = cam_desired.idx;
-        cam_info.res_x = 0;
-        cam_info.res_y = 0;
-        active_name = desired_name;
+            cap->set(cv::CAP_PROP_FRAME_WIDTH,  cam_desired.res_x);
+            cap->set(cv::CAP_PROP_FRAME_HEIGHT, cam_desired.res_y);
+            cap->set(cv::CAP_PROP_FPS, cam_desired.fps);
+
+            if (cap->isOpened())
+            {
+                cam_info.idx = cam_desired.idx;
+                cam_info.res_x = 0;
+                cam_info.res_y = 0;
+                active_name = desired_name;
+
+                return true;
+            }
+        }
     }
-    else
-        stop();
+
+    return stop(), false;
 }
 
 void Camera::stop()
@@ -119,6 +90,7 @@ void Camera::stop()
     desired_name = QString();
     active_name = QString();
     cam_info = CamInfo();
+    cam_desired = CamInfo();
 }
 
 bool Camera::_get_frame(cv::Mat* frame)
@@ -137,3 +109,13 @@ bool Camera::_get_frame(cv::Mat* frame)
     }
     return false;
 }
+
+void Camera::camera_deleter::operator()(cv::VideoCapture* cap)
+{
+    if (cap)
+    {
+        if (cap->isOpened())
+            cap->release();
+        std::default_delete<cv::VideoCapture>()(cap);
+    }
+}
diff --git a/tracker-pt/camera.h b/tracker-pt/camera.h
index 75c5faa4..3f5a8f43 100644
--- a/tracker-pt/camera.h
+++ b/tracker-pt/camera.h
@@ -7,6 +7,9 @@
 
 #pragma once
 
+#undef NDEBUG
+#include <cassert>
+
 #include "compat/util.hpp"
 
 #include <opencv2/core/core.hpp>
@@ -17,7 +20,7 @@
 
 struct CamInfo
 {
-    CamInfo() : res_x(0), res_y(0), fps(0), idx(-1) {}
+    CamInfo() : res_x(0), res_y(0), fps(-1), idx(-1) {}
 
     int res_x;
     int res_y;
@@ -29,16 +32,9 @@ class Camera final
 {
 public:
         Camera() : dt_valid(0), dt_mean(0) {}
-        ~Camera();
 
-        void start();
+        DEFUN_WARN_UNUSED bool start(int idx, int fps, int res_x, int res_y);
         void stop();
-        void restart() { stop(); start(); }
-
-        // calls corresponding template methods and reinitializes frame rate calculation
-        void set_device(const QString& name);
-        void set_fps(int fps);
-        void set_res(int x_res, int y_res);
 
         DEFUN_WARN_UNUSED bool get_frame(double dt, cv::Mat* frame);
         DEFUN_WARN_UNUSED bool get_info(CamInfo &ret);
@@ -47,9 +43,9 @@ public:
         QString get_desired_name() const;
         QString get_active_name() const;
 
-        cv::VideoCapture& operator*() { return *cap; }
-        const cv::VideoCapture& operator*() const { return *cap; }
-        cv::VideoCapture* operator->() { return cap.get(); }
+        cv::VideoCapture& operator*() { assert(cap); return *cap; }
+        const cv::VideoCapture& operator*() const { assert(cap); return *cap; }
+        cv::VideoCapture* operator->() { assert(cap); return cap.get(); }
         const cv::VideoCapture* operator->() const { return cap.get(); }
         operator bool() const { return cap && cap->isOpened(); }
 
@@ -65,19 +61,10 @@ private:
 
         struct camera_deleter final
         {
-            void operator()(cv::VideoCapture* cap)
-            {
-                if (cap)
-                {
-                    if (cap->isOpened())
-                        cap->release();
-                    static const std::default_delete<cv::VideoCapture> deleter;
-                    deleter(cap);
-                }
-            }
+            void operator()(cv::VideoCapture* cap);
         };
 
         using camera_ptr = std::unique_ptr<cv::VideoCapture, camera_deleter>;
 
-        std::unique_ptr<cv::VideoCapture, camera_deleter> cap;
+        camera_ptr cap;
 };
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 0e43b812..40293f56 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -181,22 +181,12 @@ void Tracker_PT::apply_settings()
 
     QMutexLocker l(&camera_mtx);
 
-    CamInfo info = camera.get_desired();
-    const QString name = camera.get_desired_name();
+    CamInfo info;
 
-    if (s.cam_fps != info.fps ||
-        s.cam_res_x != info.res_x ||
-        s.cam_res_y != info.res_y ||
-        s.camera_name != name)
-    {
-        qDebug() << "pt: starting camera";
-        camera.stop();
-        camera.set_device(s.camera_name);
-        camera.set_res(s.cam_res_x, s.cam_res_y);
-        camera.set_fps(s.cam_fps);
+    if (!camera.get_info(info) || frame.rows != info.res_y || frame.cols != info.res_x)
         frame = cv::Mat();
-        camera.start();
-    }
+
+    camera.start(camera_name_to_index(s.camera_name), s.cam_fps, s.cam_res_x, s.cam_res_y);
 
     qDebug() << "pt: done applying settings";
 }