tracker/pt: fix camera Hz always the default value

1 files changed, 44 insertions, 62 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);
+    }
+}