Merge branch 'unstable' into trackhattrackhat-1.2p1

4 files changed, 30 insertions, 41 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index dff0c30a..5f30c66f 100644
--- a/tracker-pt/ftnoir_tracker_pt.h
+++ b/tracker-pt/ftnoir_tracker_pt.h
@@ -41,7 +41,7 @@ public:
     void data(double* data) override;
 
     Affine pose() { return point_tracker.pose(); }
-    int  get_n_points() { return point_extractor.get_points().size(); }
+    int  get_n_points() { return point_extractor.get_n_points(); }
     bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); }
 public slots:
     void apply_settings();
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index 0208b11d..a1294c1e 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -15,6 +15,8 @@
 
 PointExtractor::PointExtractor()
 {
+    blobs.reserve(max_blobs);
+    points.reserve(max_blobs);
 }
 
 const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
@@ -34,30 +36,10 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
     const double region_size_min = s.min_point_size;
     const double region_size_max = s.max_point_size;
     
-    struct blob
-    {
-        double radius;
-        cv::Vec2d pos;
-        double confid;
-        bool taken;
-        double area;
-        blob(double radius, const cv::Vec2d& pos, double confid, double area) : radius(radius), pos(pos), confid(confid), taken(false), area(area)
-        {
-            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
-        }
-        bool inside(const blob& other)
-        {
-            cv::Vec2d tmp = pos - other.pos;
-            return sqrt(tmp.dot(tmp)) < radius;
-        }
-    };
-    
-    // mask for everything that passes the threshold (or: the upper threshold of the hysteresis)
-    
-    std::vector<blob> blobs;
-    std::vector<std::vector<cv::Point>> contours;
-
     const int thres = s.threshold;
+
+    contours.clear();
+
     if (!s.auto_threshold)
     {
         cv::threshold(frame_gray, frame_bin, thres, 255, cv::THRESH_BINARY);
@@ -95,17 +77,11 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
         cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
     }
 
-    int cnt = 0;
+    blobs.clear();
 
     for (auto& c : contours)
     {
-        if (cnt++ > 30)
-            break;
-
         const auto m = cv::moments(cv::Mat(c));
-        const double area = m.m00;
-        if (area == 0.)
-            continue;
         const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
 
         double radius;
@@ -147,9 +123,7 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
             cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
         }
 
-        blobs.push_back(blob(radius, pos, confid, area));
-        
-        enum { max_blobs = 16 };
+        blobs.push_back(blob(radius, pos, confid));
         
         if (blobs.size() == max_blobs)
             break;
@@ -158,10 +132,7 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
     using b = const blob;
     std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
     
-    points.reserve(blobs.size());
-    
     QMutexLocker l(&mtx);
-    
     points.clear();
     
     for (auto& b : blobs)
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 030251ff..3e4661f9 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -12,9 +12,10 @@
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include "ftnoir_tracker_pt_settings.h"
-
 #include <QMutex>
 
+#include <vector>
+
 class PointExtractor
 {
 public:
@@ -22,7 +23,7 @@ public:
     // dt: time since last call in seconds
     // WARNING: returned reference is valid as long as object
     const std::vector<cv::Vec2f> &extract_points(cv::Mat &frame);
-    const std::vector<cv::Vec2f>& get_points() { QMutexLocker l(&mtx); return points; }
+    int get_n_points() { QMutexLocker l(&mtx); return points.size(); }
     PointExtractor();
     
     settings_pt s;
@@ -33,6 +34,22 @@ private:
     cv::Mat frame_gray;
     cv::Mat frame_bin;
     cv::Mat hist;
+
+    enum { max_blobs = 16 };
+
+    struct blob
+    {
+        double radius;
+        cv::Vec2d pos;
+        double confid;
+        blob(double radius, const cv::Vec2d& pos, double confid) : radius(radius), pos(pos), confid(confid)
+        {
+            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
+        }
+    };
+
+    std::vector<blob> blobs;
+    std::vector<std::vector<cv::Point>> contours;
 };
 
 #endif //POINTEXTRACTOR_H
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp
index aa6feb5b..25240635 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -194,7 +194,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         float IJ0 = I0.dot(J0);
         float JJ0 = J0.dot(J0);
         float rho, theta;
-        if (JJ0 == II0) {
+        if (std::abs(JJ0 - II0) < 1e-6f) {
             rho = std::sqrt(std::abs(2*IJ0));
             theta = -PI/4;
             if (IJ0<0) theta *= -1;
@@ -202,7 +202,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
         else {
             rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 ));
             theta = atan( -2*IJ0 / (JJ0-II0) );
-            if (JJ0 - II0 < 0) theta += PI;
+            // avoid branch misprediction
+            theta += (JJ0 - II0 < 0) * PI;
             theta /= 2;
         }