1 files changed, 36 insertions, 35 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 956f639e..a85e3bc0 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -12,14 +12,15 @@
 #include <QDebug>
 #include <QFile>
 #include <QCoreApplication>
-#include "opentrack/camera-names.hpp"
+#include "opentrack-compat/camera-names.hpp"
+#include "opentrack-compat/sleep.hpp"
+#include <functional>
 
 //#define PT_PERF_LOG	//log performance
 
 //-----------------------------------------------------------------------------
 Tracker_PT::Tracker_PT()
-    : mutex(QMutex::Recursive),
-      commands(0),
+    : commands(0),
       video_widget(NULL),
       video_frame(NULL),
       ever_success(false)
@@ -34,6 +35,8 @@ Tracker_PT::~Tracker_PT()
     delete video_widget;
     video_widget = NULL;
     if (video_frame->layout()) delete video_frame->layout();
+    // fast start/stop causes breakage
+    portable::sleep(1000);
     camera.stop();
 }
 
@@ -88,44 +91,40 @@ void Tracker_PT::run()
 #endif
 
     apply_settings();
+    cv::Mat frame_;
 
     while((commands & ABORT) == 0)
     {
         const double dt = time.elapsed() * 1e-9;
         time.start();
-        cv::Mat frame;
         bool new_frame;
 
         {
             QMutexLocker l(&camera_mtx);
             new_frame = camera.get_frame(dt, &frame);
+            if (frame.rows != frame_.rows || frame.cols != frame_.cols)
+                frame_ = cv::Mat(frame.rows, frame.cols, CV_8UC3);
+            frame.copyTo(frame_);
         }
 
-        if (new_frame && !frame.empty())
+        if (new_frame && !frame_.empty())
         {
-            QMutexLocker lock(&mutex);
-
-            std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
-
-            // blobs are sorted in order of circularity
-            if (points.size() > PointModel::N_POINTS)
-                points.resize(PointModel::N_POINTS);
-
-            bool success = points.size() == PointModel::N_POINTS;
+            const auto& points = point_extractor.extract_points(frame_);
 
             float fx;
             if (!get_focal_length(fx))
                 continue;
+            
+            const bool success = points.size() >= PointModel::N_POINTS;
 
             if (success)
             {
                 point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
+                ever_success = true;
             }
             
             Affine X_CM = pose();
 
-            ever_success |= success;
-
             {
                 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
                 if (X_MH.t[0] == 0 && X_MH.t[1] == 0 && X_MH.t[2] == 0)
@@ -142,38 +141,39 @@ void Tracker_PT::run()
                     case 2: X_MH.t[0] = -135; X_MH.t[1] = 0; X_MH.t[2] = 0; break;
                     }
                 }
-                Affine X_GH = X_CM * X_MH;
-                cv::Vec3f p = X_GH.t; // head (center?) position in global space
-                cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx);  // projected to screen
-                points.push_back(p_);
             }
 
-            for (unsigned i = 0; i < points.size(); i++)
+            
+            std::function<void(const cv::Vec2f&, const cv::Scalar)> fun = [&](const cv::Vec2f& p, const cv::Scalar color)
             {
-                auto& p = points[i];
-                auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2);
-                cv::Scalar color(0, 255, 0);
-                if (i == points.size()-1)
-                    color = cv::Scalar(0, 0, 255);
-                cv::line(frame,
+                auto p2 = cv::Point(p[0] * frame_.cols + frame_.cols/2, -p[1] * frame_.cols + frame_.rows/2);
+                cv::line(frame_,
                          cv::Point(p2.x - 20, p2.y),
                          cv::Point(p2.x + 20, p2.y),
                          color,
                          4);
-                cv::line(frame,
+                cv::line(frame_,
                          cv::Point(p2.x, p2.y - 20),
                          cv::Point(p2.x, p2.y + 20),
                          color,
-                         4);
+                         4);                
+            };
+
+            for (unsigned i = 0; i < points.size(); i++)
+            {
+                fun(points[i], cv::Scalar(0, 255, 0));
+            }
+            
+            {
+                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
+                cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx);  // projected to screen
+                fun(p_, cv::Scalar(0, 0, 255));
             }
 
-            video_widget->update_image(frame);
+            video_widget->update_image(frame_);
         }
-#ifdef PT_PERF_LOG
-        log_stream<<"dt: "<<dt;
-        if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps;
-        log_stream<<"\n";
-#endif
     }
     qDebug()<<"Tracker:: Thread stopping";
 }
@@ -213,6 +213,7 @@ void Tracker_PT::apply_settings()
     camera.set_fps(cam_fps);
     qDebug() << "camera start";
     camera.start();
+    frame = cv::Mat();
     qDebug()<<"Tracker::apply ends";
 }