tracker/pt: merge from unstable

- the pose estimator doesn't need locking at all
- only return point count to the dialog, reducing locking
- allow for only 8 pixels difference between consecutive dynamic
pose frames at 640x480, half that at 320x240
- extract points taking in account pixel brightness, not merely
contours
- in case of more than three points, prefer the brightest ones
scoring on radius and average pixel brightness

6 files changed, 179 insertions, 117 deletions

diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp
index 135a8ccf..1e76af04 100755
--- a/tracker-pt/ftnoir_tracker_pt.cpp
+++ b/tracker-pt/ftnoir_tracker_pt.cpp
@@ -58,8 +58,7 @@ void Tracker_PT::reset_command(Command command)
 
 bool Tracker_PT::get_focal_length(double& ret)
 {
-    static constexpr float pi = 3.1415926;
-    float fov_;
+    int fov_;
     switch (s.fov)
     {
     default:
@@ -71,14 +70,18 @@ bool Tracker_PT::get_focal_length(double& ret)
         break;
     }
 
-    const double diag_fov = static_cast<int>(fov_) * pi / 180.f;
     QMutexLocker l(&camera_mtx);
     CamInfo info;
     const bool res = camera.get_info(info);
     if (res)
     {
+        using std::tan;
+        using std::atan;
+        using std::sqrt;
+
         const int w = info.res_x, h = info.res_y;
         const double diag = sqrt(1. + h/(double)w * h/(double)w);
+        const double diag_fov = fov_ * M_PI / 180.;
         const double fov = 2.*atan(tan(diag_fov/2.0)/diag);
         ret = .5 / tan(.5 * fov);
         return true;
@@ -88,6 +91,8 @@ bool Tracker_PT::get_focal_length(double& ret)
 
 void Tracker_PT::run()
 {
+    cv::setNumThreads(0);
+
 #ifdef PT_PERF_LOG
     QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
     if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
@@ -113,17 +118,27 @@ void Tracker_PT::run()
 
         if (new_frame && !frame_.empty())
         {
-            const auto& points = point_extractor.extract_points(frame_);
+            QMutexLocker l(&points_mtx);
+
+            point_extractor.extract_points(frame_, points);
 
             double 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);
+                const CamInfo info = camera.get_desired();
+
+                point_tracker.track(points,
+                                    PointModel(s),
+                                    fx,
+                                    s.dynamic_pose,
+                                    s.init_phase_timeout,
+                                    info.res_x,
+                                    info.res_y);
                 ever_success = true;
             }
 
@@ -131,7 +146,7 @@ void Tracker_PT::run()
 
             std::function<void(const cv::Vec2d&, const cv::Scalar&)> fun = [&](const cv::Vec2d& p, const cv::Scalar& color)
             {
-                cv::Point p2(p[0] * frame_.cols + frame_.cols/2, -p[1] * frame_.cols + frame_.rows/2);
+                cv::Point p2(int(p[0]) * frame_.cols + frame_.cols/2, -int(p[1]) * frame_.cols + frame_.rows/2);
                 cv::line(frame_,
                          cv::Point(p2.x - 20, p2.y),
                          cv::Point(p2.x + 20, p2.y),
diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h
index 613d9e82..c9936cbc 100755
--- 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_n_points(); }
+    int  get_n_points() { QMutexLocker l(&points_mtx); return int(points.size()); }
     bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); }
 public slots:
     void apply_settings();
@@ -56,9 +56,9 @@ private:
     void reset_command(Command command);
     cv::Vec3d get_model_offset();
 
-    QMutex camera_mtx;
     bool get_focal_length(double& ret);
 
+    QMutex camera_mtx, points_mtx;
     CVCamera       camera;
     PointExtractor point_extractor;
     PointTracker   point_tracker;
@@ -67,13 +67,17 @@ private:
     QFrame*      video_frame;
 
     settings_pt s;
-    Timer time;
     cv::Mat frame;
-    
+    Timer time;
+
+    std::vector<cv::Vec2d> points;
+
+    volatile int commands;
+
     volatile bool ever_success;
 
-    static constexpr double rad2deg = 180.0/3.14159265;
-    static constexpr double deg2rad = 3.14159265/180.0;
+    static constexpr double rad2deg = 180/M_PI;
+    //static constexpr float deg2rad = float(M_PI/180);
 };
 
 class TrackerDll : public Metadata
diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp
index a1294c1e..1b06df5a 100644
--- a/tracker-pt/point_extractor.cpp
+++ b/tracker-pt/point_extractor.cpp
@@ -1,5 +1,5 @@
 /* Copyright (c) 2012 Patrick Ruoff
- * Copyright (c) 2014-2015 Stanislaw Halik <sthalik@misaki.pl>
+ * Copyright (c) 2015-2016 Stanislaw Halik <sthalik@misaki.pl>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -10,24 +10,35 @@
 #include <QDebug>
 
 #ifdef DEBUG_EXTRACTION
-#   include "opentrack-compat/timer.hpp"
+#   include "compat/timer.hpp"
 #endif
 
+#include <opencv2/videoio.hpp>
+
+#include <cmath>
+#include <algorithm>
+#include <cinttypes>
+
 PointExtractor::PointExtractor()
 {
     blobs.reserve(max_blobs);
-    points.reserve(max_blobs);
 }
 
-const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
+void PointExtractor::extract_points(cv::Mat& frame, std::vector<cv::Vec2d>& points)
 {
+    using std::sqrt;
+    using std::max;
+    using std::round;
+    using std::sort;
+
     const int W = frame.cols;
     const int H = frame.rows;
-    
+
     if (frame_gray.rows != frame.rows || frame_gray.cols != frame.cols)
     {
         frame_gray = cv::Mat(frame.rows, frame.cols, CV_8U);
-        frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);;
+        frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);
+        frame_blobs = cv::Mat(frame.rows, frame.cols, CV_8U);
     }
 
     // convert to grayscale
@@ -35,15 +46,11 @@ 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;
-    
-    const int thres = s.threshold;
-
-    contours.clear();
 
     if (!s.auto_threshold)
     {
+        const int thres = s.threshold;
         cv::threshold(frame_gray, frame_bin, thres, 255, cv::THRESH_BINARY);
-        cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
     }
     else
     {
@@ -51,95 +58,107 @@ const std::vector<cv::Vec2f>& PointExtractor::extract_points(cv::Mat& frame)
                      std::vector<int> { 0 },
                      cv::Mat(),
                      hist,
-                     std::vector<int> { 256/hist_c },
-                     std::vector<float> { 0, 256/hist_c },
+                     std::vector<int> { 256 },
+                     std::vector<float> { 0, 256 },
                      false);
         const int sz = hist.cols * hist.rows;
-        int val = 0;
+        int thres = 255;
         int cnt = 0;
-        constexpr int min_pixels = 250;
-        const auto pixels_to_include = std::max<int>(0, min_pixels * s.threshold/100.);
+        constexpr double min_radius = 4;
+        constexpr double max_radius = 15;
+        const double radius = max(0., (max_radius-min_radius) * s.threshold / 256);
+        const int pixels_to_include = int((min_radius + radius)*(min_radius+radius) * 3);
         auto ptr = reinterpret_cast<const float*>(hist.ptr(0));
-        for (int i = sz-1; i >= 0; i--)
+        for (int i = sz-1; i > 0; i--)
         {
             cnt += ptr[i];
             if (cnt >= pixels_to_include)
             {
-                val = i;
+                thres = i;
                 break;
             }
         }
-        val *= hist_c;
-        val *= 240./256.;
-        //qDebug() << "val" << val;
+        //val *= 240./256.;
+        //qDebug() << "thres" << thres;
 
-        cv::threshold(frame_gray, frame_bin, val, 255, CV_THRESH_BINARY);
-        cv::findContours(frame_bin, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
+        cv::threshold(frame_gray, frame_bin, thres, 255, CV_THRESH_BINARY);
     }
 
     blobs.clear();
+    frame_bin.copyTo(frame_blobs);
 
-    for (auto& c : contours)
+    unsigned idx = 0;
+    for (int y=0; y < frame_blobs.rows; y++)
     {
-        const auto m = cv::moments(cv::Mat(c));
-        const cv::Vec2d pos(m.m10 / m.m00, m.m01 / m.m00);
+        if (idx > max_blobs) break;
 
-        double radius;
-// following based on OpenCV SimpleBlobDetector
+        const unsigned char* ptr_bin = frame_blobs.ptr(y);
+        for (int x=0; x < frame_blobs.cols; x++)
         {
-            std::vector<double> dists;
-            for (auto& k : c)
+            if (idx > max_blobs) break;
+
+            if (ptr_bin[x] != 255)
+                continue;
+            idx = blobs.size() + 1;
+            cv::Rect rect;
+            cv::floodFill(frame_blobs,
+                          cv::Point(x,y),
+                          cv::Scalar(idx),
+                          &rect,
+                          cv::Scalar(0),
+                          cv::Scalar(0),
+                          8);
+            int m00 = 0;
+            int m10 = 0;
+            int m01 = 0;
+            int cnt = 0;
+
+            for (int i=rect.y; i < (rect.y+rect.height); i++)
             {
-                dists.push_back(cv::norm(pos - cv::Vec2d(k.x, k.y)));
+                unsigned char* ptr_blobs = frame_blobs.ptr(i);
+                const unsigned char* ptr_gray = frame_gray.ptr(i);
+                for (int j=rect.x; j < (rect.x+rect.width); j++)
+                {
+                    if (ptr_blobs[j] != idx) continue;
+                    ptr_blobs[j] = 0;
+                    const int val = int(ptr_gray[j]);
+                    m00 += val;
+                    m01 += i * val;
+                    m10 += j * val;
+                    cnt++;
+                }
             }
-            std::sort(dists.begin(), dists.end());
-            radius = (dists[(dists.size() - 1)/2] + dists[dists.size()/2])/2;
-        }
-
-        if (radius < region_size_min || radius > region_size_max)
-            continue;
-
-        double confid = 1;
-        {
-            double denominator = std::sqrt(std::pow(2 * m.mu11, 2) + std::pow(m.mu20 - m.mu02, 2));
-            const double eps = 1e-2;
-            if (denominator > eps)
+            if (m00 > 0)
             {
-                double cosmin = (m.mu20 - m.mu02) / denominator;
-                double sinmin = 2 * m.mu11 / denominator;
-                double cosmax = -cosmin;
-                double sinmax = -sinmin;
-
-                double imin = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmin - m.mu11 * sinmin;
-                double imax = 0.5 * (m.mu20 + m.mu02) - 0.5 * (m.mu20 - m.mu02) * cosmax - m.mu11 * sinmax;
-                confid = imin / imax;
+                const double radius = sqrt(cnt / M_PI);
+                if (radius > region_size_max || radius < region_size_min)
+                    continue;
+                const double norm = double(m00);
+                blob b(radius, cv::Vec2d(m10 / norm, m01 / norm), m00/sqrt(double(cnt)));
+                blobs.push_back(b);
+                {
+                    char buf[64];
+                    sprintf(buf, "%.2fpx", radius);
+                    cv::putText(frame,
+                                buf,
+                                cv::Point((int)round(b.pos[0]+30), (int)round(b.pos[1]+20)),
+                                cv::FONT_HERSHEY_DUPLEX,
+                                1,
+                                cv::Scalar(0, 0, 255),
+                                1);
+                }
             }
         }
-// end SimpleBlobDetector
+    }
 
-        {
-            char buf[64];
-            sprintf(buf, "%.2fpx %.2fc", radius, confid);
-            cv::putText(frame, buf, cv::Point(pos[0]+30, pos[1]+20), cv::FONT_HERSHEY_DUPLEX, 1, cv::Scalar(0, 0, 255), 1);
-        }
+    sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) -> bool { return b2.brightness < b1.brightness; });
 
-        blobs.push_back(blob(radius, pos, confid));
-        
-        if (blobs.size() == max_blobs)
-            break;
-    }
-    
-    using b = const blob;
-    std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;});
-    
-    QMutexLocker l(&mtx);
+    points.reserve(max_blobs);
     points.clear();
-    
+
     for (auto& b : blobs)
     {
-        cv::Vec2f p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W);
+        cv::Vec2d p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W);
         points.push_back(p);
     }
-    
-    return points;
 }
diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h
index 3e4661f9..142ad60e 100644
--- a/tracker-pt/point_extractor.h
+++ b/tracker-pt/point_extractor.h
@@ -1,4 +1,5 @@
 /* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2015-2016 Stanislaw Halik <sthalik@misaki.pl>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -12,44 +13,39 @@
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include "ftnoir_tracker_pt_settings.h"
-#include <QMutex>
 
+#include <cmath>
 #include <vector>
 
-class PointExtractor
+class PointExtractor final
 {
 public:
     // extracts points from frame and draws some processing info into frame, if draw_output is set
     // 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);
-    int get_n_points() { QMutexLocker l(&mtx); return points.size(); }
+    void extract_points(cv::Mat& frame, std::vector<cv::Vec2d>& points);
     PointExtractor();
-    
+
     settings_pt s;
 private:
-    enum { hist_c = 2 };
-    std::vector<cv::Vec2f> points;
-    QMutex mtx;
+    static constexpr int max_blobs = 16;
+
     cv::Mat frame_gray;
     cv::Mat frame_bin;
     cv::Mat hist;
-
-    enum { max_blobs = 16 };
+    cv::Mat frame_blobs;
 
     struct blob
     {
-        double radius;
+        double radius, brightness;
         cv::Vec2d pos;
-        double confid;
-        blob(double radius, const cv::Vec2d& pos, double confid) : radius(radius), pos(pos), confid(confid)
+        blob(double radius, const cv::Vec2d& pos, double brightness) : radius(radius), brightness(brightness), pos(pos)
         {
-            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1] << "confid" << confid;
+            //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1];
         }
     };
 
     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 e6cda11a..c8bcb351 100644
--- a/tracker-pt/point_tracker.cpp
+++ b/tracker-pt/point_tracker.cpp
@@ -13,8 +13,6 @@
 
 #include <QDebug>
 
-const float PI = 3.14159265358979323846f;
-
 static void get_row(const cv::Matx33d& m, int i, cv::Vec3d& v)
 {
     v[0] = m(i,0);
@@ -56,12 +54,20 @@ PointTracker::PointTracker() : init_phase(true)
 {
 }
 
-PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel& model, float f)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::vector<cv::Vec2d>& points,
+                                                                     const PointModel& model,
+                                                                     double focal_length,
+                                                                     int w,
+                                                                     int h)
 {
     PointTracker::PointOrder p;
-    p.points[0] = project(cv::Vec3f(0,0,0), f);
-    p.points[1] = project(model.M01, f);
-    p.points[2] = project(model.M02, f);
+    p.points[0] = project(cv::Vec3d(0,0,0), focal_length);
+    p.points[1] = project(model.M01, focal_length);
+    p.points[2] = project(model.M02, focal_length);
+
+    const int diagonal = int(std::sqrt(w*w + h*h));
+    static constexpr int div = 100;
+    const int max_dist = diagonal / div; // 8 pixels for 640x480
 
     // set correspondences by minimum distance to projected model point
     bool point_taken[PointModel::N_POINTS];
@@ -73,7 +79,7 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
         double min_sdist = 0;
         unsigned min_idx = 0;
         // find closest point to projected model point i
-        for (int j=0; j<PointModel::N_POINTS; ++j)
+        for (unsigned j=0; j<PointModel::N_POINTS; ++j)
         {
             cv::Vec2d d = p.points[i]-points[j];
             double sdist = d.dot(d);
@@ -83,6 +89,9 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
                 min_sdist = sdist;
             }
         }
+        if (min_sdist > max_dist)
+            return find_correspondences(points, model);
+
         // if one point is closest to more than one model point, fallback
         if (point_taken[min_idx])
         {
@@ -95,7 +104,13 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
     return p;
 }
 
-void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout)
+void PointTracker::track(const std::vector<cv::Vec2d>& points,
+                         const PointModel& model,
+                         double focal_length,
+                         bool dynamic_pose,
+                         int init_phase_timeout,
+                         int w,
+                         int h)
 {
     PointOrder order;
 
@@ -108,9 +123,11 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
     if (!dynamic_pose || init_phase)
         order = find_correspondences(points, model);
     else
-        order = find_correspondences_previous(points, model, f);
+    {
+        order = find_correspondences_previous(points, model, focal_length, w, h);
+    }
 
-    POSIT(model, order, f);
+    POSIT(model, order, focal_length);
     init_phase = false;
     t.start();
 }
@@ -139,7 +156,16 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
     return p;
 }
 
-int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
+template<typename t, int h, int w>
+bool nanp(const cv::Matx<t, h, w>& m)
+{
+    for (int i = 0; i < h; i++)
+        for (int j = 0; j < w; j++)
+            if (nanp(m(i, j)))
+                return true;
+    return false;
+}
+
 bool PointTracker::POSIT(const PointModel& model, const PointOrder& order_, double focal_length)
 {
     // POSIT algorithm for coplanar points as presented in
@@ -171,6 +197,11 @@ bool PointTracker::POSIT(const PointModel& model, const PointOrder& order_, doub
     static constexpr double eps = 1e-6;
 
     const cv::Vec2d* order = order_.points;
+    using std::sqrt;
+    using std::atan;
+    using std::cos;
+    using std::sin;
+    using std::fabs;
 
     int i=1;
     for (; i<MAX_ITER; ++i)
@@ -254,7 +285,6 @@ bool PointTracker::POSIT(const PointModel& model, const PointOrder& order_, doub
         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 00e9278c..77c07125 100644
--- a/tracker-pt/point_tracker.h
+++ b/tracker-pt/point_tracker.h
@@ -15,7 +15,6 @@
 #include "ftnoir_tracker_pt_settings.h"
 
 #include <QObject>
-#include <QMutex>
 
 class Affine
 {
@@ -118,9 +117,9 @@ public:
     // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
     // 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() { QMutexLocker l(&mtx); return X_CM; }
-    cv::Vec2f project(const cv::Vec3f& v_M, float f);
+    void track(const std::vector<cv::Vec2d>& projected_points, const PointModel& model, double focal_length, bool dynamic_pose, int init_phase_timeout, int w, int h);
+    Affine pose() { return X_CM; }
+    cv::Vec2d project(const cv::Vec3d& v_M, double focal_length);
 private:
     // the points in model order
     struct PointOrder
@@ -133,15 +132,14 @@ private:
         }
     };
 
-    PointOrder find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel &model, float f);
     PointOrder find_correspondences(const std::vector<cv::Vec2d>& projected_points, const PointModel &model);
+    PointOrder find_correspondences_previous(const std::vector<cv::Vec2d>& points, const PointModel &model, double focal_length, int w, int h);
     bool POSIT(const PointModel& point_model, const PointOrder& order, double focal_length);  // The POSIT algorithm, returns the number of iterations
 
     Affine X_CM; // trafo from model to camera
 
     Timer t;
     bool init_phase;
-    QMutex mtx;
 };
 
 #endif //POINTTRACKER_H