tracker/pt: reduce mutex contention

6 files changed, 18 insertions, 20 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 877b58fd..80227a39 100644
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -19,8 +19,7 @@
 
 //-----------------------------------------------------------------------------
 Tracker_PT::Tracker_PT()
-    : mutex(QMutex::Recursive),
-      commands(0),
+    : commands(0),
       video_widget(NULL),
       video_frame(NULL),
       ever_success(false)
@@ -94,29 +93,26 @@ void Tracker_PT::run()
 
         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;
-
             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
                 Affine X_GH = X_CM * X_MH;
diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index f73d106b..5ec7ea91 100644
--- a/tracker-pt/ftnoir_tracker_pt.h
+++ b/tracker-pt/ftnoir_tracker_pt.h
@@ -40,15 +40,14 @@ public:
     void start_tracker(QFrame* parent_window) override;
     void data(double* data) override;
 
-    Affine pose() { QMutexLocker lock(&mutex); return point_tracker.pose(); }
-    int  get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); }
+    Affine pose() { return point_tracker.pose(); }
+    int  get_n_points() { return point_extractor.get_points().size(); }
     bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); }
 public slots:
     void apply_settings();
 protected:
     void run() override;
 private:
-    QMutex mutex;
     // thread commands
     enum Command {
         ABORT = 1<<0
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index dd36c57f..6eb255a5 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -147,12 +147,13 @@ std::vector<cv::Vec2f> PointExtractor::extract_points(cv::Mat& frame)
         blobs.push_back(blob(radius, pos, confid, area));
     }
     
-    // clear old points
-	points.clear();
-
     using b = const blob;
     std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
     
+    QMutexLocker l(&mtx);
+    
+	points.clear();
+    
     for (auto& b : blobs)
     {
         cv::Vec2f p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W);
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index b9368ab6..80c1897d 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -13,8 +13,8 @@
 
 #include "ftnoir_tracker_pt_settings.h"
 
-// ----------------------------------------------------------------------------
-// Extracts points from an opencv image
+#include <QMutex>
+
 class PointExtractor
 {
 public:
@@ -22,12 +22,13 @@ public:
     // dt: time since last call in seconds
     // WARNING: returned reference is valid as long as object
     std::vector<cv::Vec2f> extract_points(cv::Mat &frame);
-    const std::vector<cv::Vec2f>& get_points() { return points; }
+    const std::vector<cv::Vec2f>& get_points() { QMutexLocker l(&mtx); return points; }
     PointExtractor();
     
     settings_pt s;
 private:
     std::vector<cv::Vec2f> points;
+    QMutex mtx;
 };
 
 #endif //POINTEXTRACTOR_H
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index 924b75de..aa6feb5b 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -249,6 +249,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         old_epsilon_2 = epsilon_2;
     }
 
+    QMutexLocker l(&mtx);
     // apply results
     X_CM.R = *R_current;
     X_CM.t[0] = order[0][0] * Z0/focal_length;
diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h
index cdcf2998..2757f22c 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -15,9 +15,8 @@
 #include "ftnoir_tracker_pt_settings.h"
 
 #include <QObject>
+#include <QMutex>
 
-// ----------------------------------------------------------------------------
-// Affine frame trafo
 class Affine
 {
 public:
@@ -116,7 +115,7 @@ public:
     // f : (focal length)/(sensor width)
     // dt : time since last call
     void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout);
-    Affine pose() const { return X_CM; }
+    Affine pose() { QMutexLocker l(&mtx); return X_CM; }
     cv::Vec2f project(const cv::Vec3f& v_M, float f);
 private:
     // the points in model order
@@ -138,6 +137,7 @@ private:
 
     Timer t;
     bool init_phase;
+    QMutex mtx;
 };
 
 #endif //POINTTRACKER_H