1 files changed, 109 insertions, 114 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 243fbd60..0f8495d9 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -6,36 +6,35 @@
  * copyright notice and this permission notice appear in all copies.
  */
 
+#undef NDEBUG
 #include "ftnoir_tracker_pt.h"
-#include "compat/camera-names.hpp"
-#include "compat/math-imports.hpp"
-
 #include "pt-api.hpp"
+#include "cv/init.hpp"
+#include "video/video-widget.hpp"
+#include "compat/math-imports.hpp"
+#include "compat/check-visible.hpp"
+#include "compat/thread-name.hpp"
+#include "compat/qt-dpi.hpp"
 
-#include <cmath>
-
-#include <opencv2/imgproc.hpp>
-
+#include <cassert>
 #include <QHBoxLayout>
 #include <QDebug>
 #include <QFile>
 #include <QCoreApplication>
 
-using namespace types;
+using namespace options;
+
+namespace pt_impl {
 
-Tracker_PT::Tracker_PT(pointer<pt_runtime_traits> traits) :
+Tracker_PT::Tracker_PT(pointer<pt_runtime_traits> const& traits) :
     traits { traits },
     s { traits->get_module_name() },
     point_extractor { traits->make_point_extractor() },
-    camera { traits->make_camera() },
-    frame { traits->make_frame() },
-    preview_frame { traits->make_preview(preview_width, preview_height) }
+    frame { traits->make_frame() }
 {
-    cv::setBreakOnError(true);
+    opencv_init();
 
-    connect(s.b.get(), SIGNAL(saving()), this, SLOT(maybe_reopen_camera()), Qt::DirectConnection);
-    connect(&s.fov, SIGNAL(valueChanged(int)), this, SLOT(set_fov(int)), Qt::DirectConnection);
-    set_fov(s.fov);
+    connect(&*s.b, &bundle_::saving, this, [this]{ reopen_camera_flag = true; }, Qt::DirectConnection);
 }
 
 Tracker_PT::~Tracker_PT()
@@ -43,111 +42,108 @@ Tracker_PT::~Tracker_PT()
     requestInterruption();
     wait();
 
-    QMutexLocker l(&camera_mtx);
-    camera->stop();
+    if (camera)
+        camera->stop();
 }
 
-void Tracker_PT::run()
+bool Tracker_PT::check_camera()
 {
-    cv::setNumThreads(1);
+    if (reopen_camera_flag)
+    {
+        reopen_camera_flag = false;
+
+        camera = nullptr;
+        camera = traits->make_camera();
+        if (!camera || !camera->start(s))
+            return false;
+    }
+    assert(camera);
+    if (progn(bool x = true; return open_camera_dialog_flag.compare_exchange_strong(x, false);))
+        run_in_thread_sync(qApp->thread(), [this] { camera->show_camera_settings(); });
+    return true;
+}
 
-#ifdef PT_PERF_LOG
-    QFile log_file(OPENTRACK_BASE_PATH + "/PointTrackerPerformance.txt");
-    if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
-    QTextStream log_stream(&log_file);
-#endif
+void Tracker_PT::run()
+{
+    portable::set_curthread_name("tracker/pt");
 
     while(!isInterruptionRequested())
     {
+        if (!check_camera())
+            break;
+
         pt_camera_info info;
         bool new_frame = false;
 
         {
-            QMutexLocker l(&camera_mtx);
-
-            if (camera)
-                std::tie(new_frame, info) = camera->get_frame(*frame);
+            camera->set_fov(s.fov);
+            std::tie(new_frame, info) = camera->get_frame(*frame);
         }
 
         if (new_frame)
         {
-            *preview_frame = *frame;
+            const bool preview_visible = check_is_visible();
 
-            point_extractor->extract_points(*frame, *preview_frame, points);
-            point_count = points.size();
+            if (preview_visible && !widget->fresh())
+                preview_frame->set_last_frame(*frame);
 
-            const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
+            point_extractor->extract_points(*frame, *preview_frame, preview_visible && !widget->fresh(), points);
+            point_count.store(points.size(), std::memory_order_relaxed);
 
             const bool success = points.size() >= PointModel::N_POINTS;
 
-            if (success)
-            {
-                point_tracker.track(points,
-                                    PointModel(s),
-                                    info,
-                                    s.dynamic_pose ? s.init_phase_timeout : 0);
-                ever_success = true;
-            }
+            Affine X_CM;
 
             {
-                Affine X_CM;
+                QMutexLocker l(&center_lock);
+
+                if (success)
                 {
-                    QMutexLocker l(&data_mtx);
-                    X_CM = point_tracker.pose();
+                    int dynamic_pose_ms = s.dynamic_pose ? s.init_phase_timeout : 0;
+
+                    point_tracker.track(points, PointModel(s), info, dynamic_pose_ms, filter, camera->deadzone_amount());
+                    ever_success.store(true, std::memory_order_relaxed);
                 }
 
-                // just copy pasted these lines from below
+                QMutexLocker l2(&data_lock);
+                X_CM = point_tracker.pose();
+            }
+
+            if (preview_visible && !widget->fresh())
+            {
+                const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y);
                 Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z));
                 Affine X_GH = X_CM * X_MH;
                 vec3 p = X_GH.t; // head (center?) position in global space
 
-                preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
-            }
-
-            video_widget->update_image(preview_frame->get_bitmap());
+                if (p[2] > f(.1))
+                    preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]);
 
-            {
-                int w = -1, h = -1;
-                video_widget->get_preview_size(w, h);
-                if (w != preview_width || h != preview_height)
-                {
-                    preview_width = w, preview_height = h;
-                    preview_frame = traits->make_preview(w, h);
-                }
+                widget->update_image(preview_frame->get_bitmap());
             }
         }
     }
-    qDebug() << "pt: thread stopped";
-}
-
-bool Tracker_PT::maybe_reopen_camera()
-{
-    QMutexLocker l(&camera_mtx);
-
-    return camera->start(camera_name_to_index(s.camera_name),
-                         s.cam_fps, s.cam_res_x, s.cam_res_y);
-}
-
-void Tracker_PT::set_fov(int value)
-{
-    QMutexLocker l(&camera_mtx);
-    camera->set_fov(value);
 }
 
 module_status Tracker_PT::start_tracker(QFrame* video_frame)
 {
-    //video_frame->setAttribute(Qt::WA_NativeWindow);
+    {
+        auto camera = traits->make_camera();
+        if (!camera || !camera->start(s))
+            return error(tr("Failed to open camera '%1'").arg(s.camera_name));
+    }
 
-    video_widget = std::make_unique<cv_video_widget>(video_frame);
+    widget = std::make_unique<video_widget>(video_frame);
     layout = std::make_unique<QHBoxLayout>(video_frame);
     layout->setContentsMargins(0, 0, 0, 0);
-    layout->addWidget(video_widget.get());
-    video_frame->setLayout(layout.get());
+    layout->addWidget(&*widget);
+    video_frame->setLayout(&*layout);
     //video_widget->resize(video_frame->width(), video_frame->height());
     video_frame->show();
 
-    if (!maybe_reopen_camera())
-        return { tr("Can't open camera") };
+    double dpi = screen_dpi(video_frame);
+    preview_frame = traits->make_preview(iround(preview_width * dpi),
+                                         iround(preview_height * dpi));
 
     start(QThread::HighPriority);
 
@@ -156,76 +152,75 @@ module_status Tracker_PT::start_tracker(QFrame* video_frame)
 
 void Tracker_PT::data(double *data)
 {
-    if (ever_success)
+    if (ever_success.load(std::memory_order_relaxed))
     {
-        Affine X_CM = pose();
+        Affine X_CM;
+        {
+            QMutexLocker l(&data_lock);
+            X_CM = point_tracker.pose();
+        }
 
         Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z));
-        Affine X_GH = X_CM * X_MH;
+        Affine X_GH(X_CM * X_MH);
 
         // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
         // -z -> x, y -> z, x -> -y
         mat33 R_EG(0, 0,-1,
                    -1, 0, 0,
                    0, 1, 0);
-        mat33 R = R_EG *  X_GH.R * R_EG.t();
+        mat33 R(R_EG *  X_GH.R * R_EG.t());
 
         // get translation(s)
         const vec3& t = X_GH.t;
 
         // extract rotation angles
-        {
-            f alpha, beta, gamma;
-            beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
-            alpha = atan2( R(1,0), R(0,0));
-            gamma = atan2( R(2,1), R(2,2));
+        auto r00 = (double)R(0, 0);
+        auto r10 = (double)R(1,0), r20 = (double)R(2,0);
+        auto r21 = (double)R(2,1), r22 = (double)R(2,2);
 
-#if 0
-            if (t[2] > 1e-4)
-            {
-                alpha += copysign(atan(t[0] / t[2]), t[0]);
-                // pitch is skewed anyway due to only one focal length value
-                //beta -= copysign(atan(t[1] / t[2]), t[1]);
-            }
-#endif
+        double beta  = atan2(-r20, sqrt(r21*r21 + r22*r22));
+        double alpha = atan2(r10, r00);
+        double gamma = atan2(r21, r22);
 
-            data[Yaw] = rad2deg * alpha;
-            data[Pitch] = -rad2deg * beta;
-            data[Roll] = rad2deg * gamma;
-        }
+        constexpr double rad2deg = 180/M_PI;
+
+        data[Yaw]   = rad2deg * alpha;
+        data[Pitch] = -rad2deg * beta;
+        data[Roll]  = rad2deg * gamma;
 
         // convert to cm
-        data[TX] = t[0] / 10;
-        data[TY] = t[1] / 10;
-        data[TZ] = t[2] / 10;
+        data[TX] = (double)t[0] / 10;
+        data[TY] = (double)t[1] / 10;
+        data[TZ] = (double)t[2] / 10;
     }
 }
 
 bool Tracker_PT::center()
 {
+    QMutexLocker l(&center_lock);
+
     point_tracker.reset_state();
     return false;
 }
 
-Affine Tracker_PT::pose()
-{
-    QMutexLocker l(&data_mtx);
-
-    return point_tracker.pose();
-}
-
 int Tracker_PT::get_n_points()
 {
-    return int(point_count);
+    return (int)point_count.load(std::memory_order_relaxed);
 }
 
-bool Tracker_PT::get_cam_info(pt_camera_info* info)
+bool Tracker_PT::get_cam_info(pt_camera_info& info)
 {
-    QMutexLocker lock(&camera_mtx);
-    bool ret;
+    bool ret = false;
 
-    std::tie(ret, *info) = camera->get_info();
+    if (camera)
+        std::tie(ret, info) = camera->get_info();
     return ret;
 }
 
+Affine Tracker_PT::pose() const
+{
+    QMutexLocker l(&data_lock);
+    return point_tracker.pose();
+}
 
+} // ns pt_impl