pt: stop crashing on camera ops

5 files changed, 42 insertions, 61 deletions

diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp
index 9e11b815..78a37b71 100644
--- a/ftnoir_tracker_pt/camera.cpp
+++ b/ftnoir_tracker_pt/camera.cpp
@@ -43,14 +43,14 @@ void Camera::set_res(int x_res, int y_res)
     }
 }
 
-CamInfo Camera::get_info()
+bool Camera::get_info(CamInfo& ret)
 {
     if (cam_info.res_x == 0 || cam_info.res_y == 0)
     {
-        cv::Mat tmp;
-        _get_frame(&tmp);
+        return false;
     }
-    return cam_info;
+    ret = cam_info;
+    return true;
 }
 
 bool Camera::get_frame(float dt, cv::Mat* frame)
@@ -65,26 +65,25 @@ bool Camera::get_frame(float dt, cv::Mat* frame)
         cam_info.fps = dt_mean > 1e-3 ? 1.0 / dt_mean : 0;
         dt_valid = 0;
     }
+    else
+        qDebug() << "pt camera: can't get frame";
     return new_frame;
 }
 
 void CVCamera::start()
 {
-    if (cap)
-        delete cap;
+    stop();
     cap = new cv::VideoCapture(desired_index);
     _set_res();
     _set_fps();
     // extract camera info
     if (cap->isOpened())
     {
-        active = true;
         active_index = desired_index;
         cam_info.res_x = 0;
         cam_info.res_y = 0;
     } else {
-        delete cap;
-        cap = nullptr;
+        stop();
     }
 }
 
@@ -92,13 +91,19 @@ void CVCamera::stop()
 {
     if (cap)
     {
-        cap->release();
+        const bool opened = cap->isOpened();
+        if (opened)
+        {
+            qDebug() << "pt: freeing camera";
+            cap->release();
+        }
         delete cap;
         cap = nullptr;
         // give opencv time to exit camera threads, etc.
-        portable::sleep(500);
+        if (opened)
+            portable::sleep(500);
+        qDebug() << "camera: assuming stopped";
     }
-    active = false;
 }
 
 bool CVCamera::_get_frame(cv::Mat* frame)
@@ -135,10 +140,6 @@ void CVCamera::_set_res()
 }
 void CVCamera::_set_device_index()
 {
-    if (cap)
-    {
-        cap->release();
-        delete cap;
-    }
-    cap = new cv::VideoCapture(desired_index);
+    if (desired_index != active_index)
+        stop();
 }
diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h
index 343446ac..e73d9dff 100644
--- a/ftnoir_tracker_pt/camera.h
+++ b/ftnoir_tracker_pt/camera.h
@@ -26,7 +26,7 @@ struct CamInfo
 class Camera
 {
 public:
-        Camera() : dt_valid(0), dt_mean(0), desired_index(0), active_index(-1), active(false) {}
+        Camera() : dt_valid(0), dt_mean(0), desired_index(0), active_index(-1) {}
         virtual ~Camera() = 0;
 
         // start/stop capturing
@@ -43,7 +43,7 @@ public:
         bool get_frame(float dt, cv::Mat* frame);
 
         // WARNING: returned references are valid as long as object
-        CamInfo get_info();
+        bool get_info(CamInfo &ret);
         CamInfo get_desired() const { return cam_desired; }
 
 protected:
@@ -60,7 +60,6 @@ private:
 protected:
         int desired_index;
         int active_index;
-        bool active;
         CamInfo cam_info;
         CamInfo cam_desired;
 };
@@ -68,7 +67,6 @@ inline Camera::~Camera() {}
 
 // ----------------------------------------------------------------------------
 // camera based on OpenCV's videoCapture
-#ifdef OPENTRACK_API
 class CVCamera : public Camera
 {
 public:
@@ -88,29 +86,6 @@ protected:
 private:
     cv::VideoCapture* cap;
 };
-#else
-// ----------------------------------------------------------------------------
-// Camera based on the videoInput library
-class VICamera : public Camera
-{
-public:
-    VICamera();
-    ~VICamera() { stop(); }
-
-    virtual void start();
-    virtual void stop();
-
-protected:
-    virtual bool _get_frame(cv::Mat* frame);
-    virtual void _set_device_index();
-    virtual void _set_fps();
-    virtual void _set_res();
-
-    videoInput VI;
-    cv::Mat new_frame;
-    unsigned char* frame_buffer;
-};
-#endif
 
 enum RotationType
 {
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 07600646..9cfd31eb 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -48,7 +48,7 @@ void Tracker_PT::reset_command(Command command)
 	commands &= ~command;
 }
 
-float Tracker_PT::get_focal_length()
+bool Tracker_PT::get_focal_length(float& ret)
 {
     static constexpr float pi = 3.1415926;
     float fov_;
@@ -65,11 +65,17 @@ float Tracker_PT::get_focal_length()
 
     const float diag_fov = static_cast<int>(fov_) * pi / 180.f;
     QMutexLocker l(&camera_mtx);
-    CamInfo info = camera.get_info();
-    const int w = info.res_x, h = info.res_y;
-    const double diag = sqrt(w * w + h * h)/w;
-    const double fov = 2.*atan(tan(diag_fov/2.0)/sqrt(1. + diag*diag));
-    return .5 / tan(.5 * fov);
+    CamInfo info;
+    const bool res = camera.get_info(info);
+    if (res)
+    {
+        const int w = info.res_x, h = info.res_y;
+        const double diag = sqrt(w * w + h * h)/w;
+        const double fov = 2.*atan(tan(diag_fov/2.0)/sqrt(1. + diag*diag));
+        ret = .5 / tan(.5 * fov);
+        return true;
+    }
+    return false;
 }
 
 void Tracker_PT::run()
@@ -103,10 +109,14 @@ void Tracker_PT::run()
             bool success = points.size() == PointModel::N_POINTS;
             
             ever_success |= success;
+
+            float fx;
+            if (!get_focal_length(fx))
+                continue;
             
             if (success)
             {
-                point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose, s.init_phase_timeout);
+                point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
             }
             
             {
@@ -114,7 +124,6 @@ void Tracker_PT::run()
                 Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
                 Affine X_GH = X_CM * X_MH;
                 cv::Vec3f p = X_GH.t; // head (center?) position in global space
-                float fx = get_focal_length();
                 cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx);  // projected to screen
                 points.push_back(p_);
             }
@@ -153,7 +162,6 @@ void Tracker_PT::apply_settings()
 {
     qDebug()<<"Tracker:: Applying settings";
     QMutexLocker l(&camera_mtx);
-    camera.stop();
     camera.set_device_index(camera_name_to_index("PS3Eye Camera"));
     int res_x, res_y, cam_fps;
     switch (s.camera_mode)
@@ -183,6 +191,7 @@ void Tracker_PT::apply_settings()
 
     camera.set_res(res_x, res_y);
     camera.set_fps(cam_fps);
+    qDebug() << "camera start";
     camera.start();
     qDebug()<<"Tracker::apply ends";
 }
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 3291c7fc..f73d106b 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -42,7 +42,7 @@ public:
 
     Affine pose() { QMutexLocker lock(&mutex); return point_tracker.pose(); }
     int  get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); }
-    void get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); *info = camera.get_info(); }
+    bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); }
 public slots:
     void apply_settings();
 protected:
@@ -56,7 +56,7 @@ private:
     void set_command(Command command);
     void reset_command(Command command);
     
-    float get_focal_length();
+    bool get_focal_length(float &ret);
     
     volatile int commands;
 
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index 2379de41..1474ad40 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -9,11 +9,7 @@
 #define POINTTRACKER_H
 
 #include <opencv2/core/core.hpp>
-#ifndef OPENTRACK_API
-#   include <boost/shared_ptr.hpp>
-#else
-#   include <memory>
-#endif
+#include <memory>
 #include <vector>
 #include "opentrack-compat/timer.hpp"
 #include "ftnoir_tracker_pt_settings.h"