pt: stop crashing on camera ops

1 files changed, 18 insertions, 9 deletions

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";
 }