aruco cleanup, experimental contrast fix

7 files changed, 209 insertions, 150 deletions

diff --git a/ftnoir_tracker_aruco/ar_video_widget.cpp b/ftnoir_tracker_aruco/ar_video_widget.cpp
index 9a089213..61a611ea 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.cpp
+++ b/ftnoir_tracker_aruco/ar_video_widget.cpp
@@ -40,3 +40,15 @@ void ArucoVideoWidget::update_and_repaint()
     texture = qframe2;
     update();
 }
+
+void ArucoVideoWidget::paintEvent(QPaintEvent* e)
+{
+    QMutexLocker foo(&mtx);
+    QPainter(this).drawImage(e->rect(), texture);
+}
+
+ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent)
+{
+    connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
+    timer.start(60);
+}
\ No newline at end of file
diff --git a/ftnoir_tracker_aruco/ar_video_widget.h b/ftnoir_tracker_aruco/ar_video_widget.h
index e2cf4d9f..820ba7d0 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.h
+++ b/ftnoir_tracker_aruco/ar_video_widget.h
@@ -22,26 +22,18 @@
 class ArucoVideoWidget : public QWidget
 {
 	Q_OBJECT
-
-public:
-    ArucoVideoWidget(QWidget *parent) : QWidget(parent) {
-        connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
-        timer.start(60);
-	}
-    void update_image(const cv::Mat& frame);
-protected slots:
-    void paintEvent( QPaintEvent* e ) {
-        QMutexLocker foo(&mtx);
-        QPainter painter(this);
-        painter.drawImage(e->rect(), texture);
-    }
-    void update_and_repaint();
-
+    
 private:
     QMutex mtx;
     QImage texture;
     QTimer timer;
     cv::Mat _frame;
+private slots:
+    void update_and_repaint(); 
+public:
+    ArucoVideoWidget(QWidget *parent);
+    void update_image(const cv::Mat& frame);   
+    void paintEvent( QPaintEvent*) override;
 };
 
 #endif // VIDEOWIDGET_H
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 240ef5f8..1898d15b 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -9,7 +9,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>560</width>
+    <width>562</width>
     <height>214</height>
    </rect>
   </property>
@@ -40,35 +40,6 @@
         </property>
        </widget>
       </item>
-      <item row="0" column="1">
-       <widget class="QDoubleSpinBox" name="cameraFOV">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="locale">
-         <locale language="English" country="UnitedStates"/>
-        </property>
-        <property name="minimum">
-         <double>35.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>180.000000000000000</double>
-        </property>
-        <property name="value">
-         <double>52.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_2">
-        <property name="text">
-         <string>Frames per second</string>
-        </property>
-       </widget>
-      </item>
       <item row="1" column="1">
        <widget class="QComboBox" name="cameraFPS">
         <property name="sizePolicy">
@@ -104,10 +75,39 @@
         </item>
        </widget>
       </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_3">
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_4">
         <property name="text">
-         <string>Camera name</string>
+         <string>Resolution</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QDoubleSpinBox" name="cameraFOV">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="locale">
+         <locale language="English" country="UnitedStates"/>
+        </property>
+        <property name="minimum">
+         <double>35.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>180.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>52.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>Frames per second</string>
         </property>
        </widget>
       </item>
@@ -121,10 +121,10 @@
         </property>
        </widget>
       </item>
-      <item row="3" column="0">
-       <widget class="QLabel" name="label_4">
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_3">
         <property name="text">
-         <string>Resolution</string>
+         <string>Camera name</string>
         </property>
        </widget>
       </item>
@@ -159,16 +159,25 @@
        </widget>
       </item>
       <item row="4" column="0">
-       <widget class="QLabel" name="label_10">
+       <widget class="QLabel" name="label_6">
         <property name="text">
-         <string>Red channel only</string>
+         <string>Sun glare removal</string>
         </property>
        </widget>
       </item>
       <item row="4" column="1">
-       <widget class="QCheckBox" name="red_only">
-        <property name="text">
-         <string>Mileage may vary</string>
+       <widget class="QSlider" name="desaturate_slider">
+        <property name="maximum">
+         <number>100</number>
+        </property>
+        <property name="singleStep">
+         <number>10</number>
+        </property>
+        <property name="pageStep">
+         <number>20</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
         </property>
        </widget>
       </item>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 31aa2372..0974f0f0 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -8,6 +8,7 @@
 #include <vector>
 #include <cstdio>
 #include <cmath>
+#include <algorithm>
 #include <QMutexLocker>
 #include "./include/markerdetector.h"
 #include "ftnoir_tracker_aruco.h"
@@ -135,7 +136,7 @@ void Tracker::StartTracker(QFrame* videoframe)
 
 #define HT_PI 3.1415926535
 
-void Tracker::getRT(cv::Matx33f& r_, cv::Vec3f& t_)
+void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
 {
     QMutexLocker l(&mtx);
 
@@ -183,10 +184,6 @@ void Tracker::run()
 
     cv::Rect last_roi(65535, 65535, 0, 0);
 
-    cv::Mat color, color_, grayscale, rvec, tvec;
-
-    const double stateful_coeff = 0.88;
-
     if (!camera.isOpened())
     {
         fprintf(stderr, "aruco tracker: can't open camera\n");
@@ -201,19 +198,46 @@ void Tracker::run()
 
     while (!stop)
     {
-        if (!camera.read(color_))
+        cv::Mat color;
+        if (!camera.read(color))
             continue;
         auto tm = cv::getTickCount();
-        color_.copyTo(color);
-        if (s.red_only)
+        
+        const double c = s.desaturate * 16. / 100.;
+        
+        if (std::abs(c) > 1e-3)
         {
-            cv::Mat channel[3];
-            cv::split(color, channel);
-            grayscale = channel[2];
-        } else
-            cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
-
-        gain.tick(camera, grayscale);
+            const int w=color.cols, h=color.rows;
+            
+            cv::Mat hsv;
+            cv::cvtColor(color, hsv, cv::COLOR_BGR2HSV);
+            vector<cv::Mat> channels;
+            cv::split(hsv, channels);
+            cv::Mat sat = channels[1];
+            cv::Mat val = channels[2];
+            
+            struct ops {
+                static double sig(double x)
+                {
+                    double x_ = -6 + x * 2 * 6;
+                    return 1./(1.+exp(-x_));
+                }
+            };
+            
+            for (int i = 0; i < h; i++)
+                for (int j = 0; j < w; j++)
+                {
+                    const double sat_ij = sat.at<unsigned char>(i, j)/255.;
+                    val.at<unsigned char>(i, j) *= std::max(0., 1. - c*ops::sig(sat_ij));
+                }
+            
+            channels[1] = sat;
+            channels[2] = val;
+            cv::merge(channels, hsv);
+            cv::cvtColor(hsv, color, cv::COLOR_HSV2BGR);
+        }
+        cv::Mat grayscale;
+        cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
 
         const int scale = frame.cols > 480 ? 2 : 1;
         detector.setThresholdParams(scale > 1 ? 11 : 7, 4);
@@ -232,27 +256,33 @@ void Tracker::run()
 
         const double size_min = 0.04;
         const double size_max = 0.38;
-
-        if (last_roi.width > 0 &&
-                (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
-                 markers.size() == 1 && markers[0].size() == 4))
+        
+        bool roi_valid = false;
+        
+        if (last_roi.width > 0 && last_roi.height)
         {
             detector.setMinMaxSize(std::max(0.01, size_min * grayscale.cols / last_roi.width),
                                    std::min(1.0, size_max * grayscale.cols / last_roi.width));
-            auto& m = markers.at(0);
-            for (int i = 0; i < 4; i++)
+            if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
+                markers.size() == 1 && markers[0].size() == 4)
             {
-                auto& p = m.at(i);
-                p.x += last_roi.x;
-                p.y += last_roi.y;
+                auto& m = markers.at(0);
+                for (int i = 0; i < 4; i++)
+                {
+                    auto& p = m.at(i);
+                    p.x += last_roi.x;
+                    p.y += last_roi.y;
+                }
+                roi_valid = true;
             }
         }
-        else
+        
+        if (!roi_valid)
         {
             detector.setMinMaxSize(size_min, size_max);
             detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
         }
-
+        
         if (markers.size() == 1 && markers[0].size() == 4) {
             const auto& m = markers.at(0);
             for (int i = 0; i < 4; i++)
@@ -275,70 +305,89 @@ void Tracker::run()
         frame = color.clone();
 
         ::sprintf(buf, "Hz: %d", last_fps);
-        cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
+        cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), 1);
         ::sprintf(buf, "Jiffies: %ld", (long) (10000 * (time - tm) / freq));
-        cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
+        cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), 1);
 
         if (markers.size() == 1 && markers[0].size() == 4) {
             const auto& m = markers.at(0);
             const float size = 40;
 
             cv::Mat obj_points(4,3,CV_32FC1);
-            obj_points.at<float>(1,0)=-size + s.headpos_x;
-            obj_points.at<float>(1,1)=-size + s.headpos_y;
-            obj_points.at<float>(1,2)=-size + s.headpos_z;
-            obj_points.at<float>(2,0)=size + s.headpos_x;
-            obj_points.at<float>(2,1)=-size + s.headpos_y;
-            obj_points.at<float>(2,2)=-size + s.headpos_z;
-            obj_points.at<float>(3,0)=size + s.headpos_x;
-            obj_points.at<float>(3,1)=size + s.headpos_y;
-            obj_points.at<float>(3,2)=size + s.headpos_z;
-            obj_points.at<float>(0,0)=-size + s.headpos_x;
-            obj_points.at<float>(0,1)=size + s.headpos_y;
-            obj_points.at<float>(0,2)=size + s.headpos_z;
-
-            last_roi = cv::Rect(65535, 65535, 0, 0);
-
+            const int x1=1, x2=2, x3=3, x4=0;
+            obj_points.at<float>(x1,0)=-size + s.headpos_x;
+            obj_points.at<float>(x1,1)=-size + s.headpos_y;
+            obj_points.at<float>(x1,2)= 0 + s.headpos_z;
+            
+            obj_points.at<float>(x2,0)=size + s.headpos_x;
+            obj_points.at<float>(x2,1)=-size + s.headpos_y;
+            obj_points.at<float>(x2,2)= 0 + s.headpos_z;
+            
+            obj_points.at<float>(x3,0)=size + s.headpos_x;
+            obj_points.at<float>(x3,1)=size + s.headpos_y;
+            obj_points.at<float>(x3,2)= 0 + s.headpos_z;
+            
+            obj_points.at<float>(x4,0)= -size + s.headpos_x;
+            obj_points.at<float>(x4,1)= size + s.headpos_y;
+            obj_points.at<float>(x4,2)= 0 + s.headpos_z;
+            
+            cv::Vec3d rvec, tvec;
+            
+            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
+            
+            std::vector<cv::Point2f> roi_projection(4);
+            cv::Mat roi_points = obj_points * c_search_window;
+            cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
+            
+            last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0);
+            
             for (int i = 0; i < 4; i++)
             {
-                auto foo = m.at(i);
-                last_roi.x = std::min<int>(foo.x, last_roi.x);
-                last_roi.y = std::min<int>(foo.y, last_roi.y);
-                last_roi.width = std::max<int>(foo.x, last_roi.width);
-                last_roi.height = std::max<int>(foo.y, last_roi.height);
-            }
-            {
-                last_roi.width -= last_roi.x;
-                last_roi.height -= last_roi.y;
-                last_roi.x -= last_roi.width * stateful_coeff;
-                last_roi.y -= last_roi.height * stateful_coeff;
-                last_roi.width *= stateful_coeff * 3;
-                last_roi.height *= stateful_coeff * 3;
-                last_roi.x = std::max<int>(0, last_roi.x);
-                last_roi.y = std::max<int>(0, last_roi.y);
-                last_roi.width = std::min<int>(grayscale.cols - last_roi.x, last_roi.width);
-                last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height);
+                auto proj = roi_projection[i];
+                int min_x = std::min<int>(proj.x, last_roi.x),
+                    min_y = std::min<int>(proj.y, last_roi.y);
+                
+                int max_x = std::max<int>(proj.x, last_roi.width),
+                    max_y = std::max<int>(proj.y, last_roi.height);
+                
+                last_roi.x = min_x;
+                last_roi.y = min_y;
+                
+                last_roi.width = max_x;
+                last_roi.height = max_y;
             }
-
-            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
-            cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1);
-            cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1);
-            cv::Rodrigues(rvec, rotation_matrix);
+            
+            if (last_roi.x < 0)
+                last_roi.x = 0;
+            if (last_roi.y < 0)
+                last_roi.y = 0;
+            
+            if (last_roi.width+1 > color.cols)
+                last_roi.width = color.cols-1;
+            
+            if (last_roi.height+1 > color.rows)
+                last_roi.height = color.rows-1;
+
+            last_roi.width -= last_roi.x;
+            last_roi.height -= last_roi.y;
+          
+            auto rmat = cv::Matx33d::zeros();
+            cv::Matx33d m_r(3, 3, CV_64FC1), m_q(3, 3, CV_64FC1);
+            cv::Rodrigues(rvec, rmat);
 
             {
-                cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
+                cv::Vec3d euler = cv::RQDecomp3x3(rmat, m_r, m_q);
 
                 QMutexLocker lck(&mtx);
 
                 for (int i = 0; i < 3; i++)
-                    pose[i] = tvec.at<double>(i);
-
+                    pose[i] = tvec(i);
                 pose[Yaw] = euler[1];
                 pose[Pitch] = -euler[0];
                 pose[Roll] = euler[2];
 
-                rotation_matrix.convertTo(r, CV_32FC1);
-                tvec.convertTo(t, CV_32FC1);
+                r = rmat;
+                t = tvec;
             }
 
             std::vector<cv::Point2f> repr2;
@@ -350,6 +399,9 @@ void Tracker::run()
                 auto s = cv::Scalar(255, 0, 255);
                 cv::circle(frame, repr2.at(0), 4, s, -1);
             }
+            
+            if (roi_valid)
+                cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1);
 
             last_centroid = repr2[0];
         }
@@ -438,7 +490,7 @@ TrackerControls::TrackerControls()
     tie_setting(s.headpos_x, ui.cx);
     tie_setting(s.headpos_y, ui.cy);
     tie_setting(s.headpos_z, ui.cz);
-    tie_setting(s.red_only, ui.red_only);
+    tie_setting(s.desaturate, ui.desaturate_slider);
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
     connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate()));
@@ -467,8 +519,8 @@ void TrackerControls::update_tracker_calibration()
 {
     if (calib_timer.isActive() && tracker)
     {
-        cv::Matx33f r;
-        cv::Vec3f t;
+        cv::Matx33d r;
+        cv::Vec3d t;
         tracker->getRT(r, t);
         calibrator.update(r, t);
         auto pos = calibrator.get_estimate() * .1;
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index c53a49a4..03fff844 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -5,8 +5,7 @@
  * copyright notice and this permission notice appear in all copies.
  */
 
-#ifndef FTNOIR_TRACKER_HT_H
-#define FTNOIR_TRACKER_HT_H
+#pragma once
 
 #include "ui_aruco-trackercontrols.h"
 #include "ar_video_widget.h"
@@ -19,7 +18,9 @@
 #include "facetracknoir/options.h"
 #include "ftnoir_tracker_aruco/trans_calib.h"
 #include "facetracknoir/plugin-api.hpp"
-#include "facetracknoir/gain-control.hpp"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
 
 using namespace options;
 
@@ -27,7 +28,7 @@ struct settings {
     pbundle b;
     value<double> fov, headpos_x, headpos_y, headpos_z;
     value<int> camera_index, force_fps, resolution;
-    value<bool> red_only;
+    value<int> desaturate;
     settings() :
         b(bundle("aruco-tracker")),
         fov(b, "field-of-view", 56),
@@ -37,13 +38,14 @@ struct settings {
         camera_index(b, "camera-index", 0),
         force_fps(b, "force-fps", 0),
         resolution(b, "force-resolution", 0),
-        red_only(b, "red-only", false)
+        desaturate(b, "desaturate", 0)
     {}
 };
 
 class Tracker : protected QThread, public ITracker
 {
     Q_OBJECT
+    static constexpr double c_search_window = 2.9;
 public:
     Tracker();
     ~Tracker() override;
@@ -51,7 +53,7 @@ public:
     void GetHeadPoseData(double *data);
     void run();
     void reload() { s.b->reload(); }
-    void getRT(cv::Matx33f& r, cv::Vec3f& t);
+    void getRT(cv::Matx33d &r, cv::Vec3d &t);
 private:
     QMutex mtx;
     volatile bool stop;
@@ -61,9 +63,8 @@ private:
     double pose[6];
     cv::Mat frame;
     cv::VideoCapture camera;
-    cv::Matx33f r;
-    cv::Vec3f t;
-    Gain gain;
+    cv::Matx33d r;
+    cv::Vec3d t;
 };
 
 class TrackerControls : public QWidget, public ITrackerDialog
@@ -71,12 +72,8 @@ class TrackerControls : public QWidget, public ITrackerDialog
     Q_OBJECT
 public:
     TrackerControls();
-    void registerTracker(ITracker * x) {
-        tracker = dynamic_cast<Tracker*>(x);
-    }
-    void unRegisterTracker() {
-        tracker = nullptr;
-    }
+    void registerTracker(ITracker * x) { tracker = dynamic_cast<Tracker*>(x); }
+    void unRegisterTracker() { tracker = nullptr; }
 private:
     Ui::Form ui;
     Tracker* tracker;
@@ -90,6 +87,3 @@ private slots:
     void cleanupCalib();
     void update_tracker_calibration();
 };
-
-#endif
-
diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp
index b1f956b4..369de449 100644
--- a/ftnoir_tracker_aruco/trans_calib.cpp
+++ b/ftnoir_tracker_aruco/trans_calib.cpp
@@ -21,9 +21,9 @@ void TranslationCalibrator::reset()
 	y = Vec6f(0,0,0, 0,0,0);
 }
 
-void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k)
+void TranslationCalibrator::update(const Matx33d& R_CM_k, const Vec3d& t_CM_k)
 {
-	Matx<float, 6,3> H_k_T = Matx<float, 6,3>::zeros();
+	Matx<double, 6,3> H_k_T = Matx<double, 6,3>::zeros();
 	for (int i=0; i<3; ++i) {
 		for (int j=0; j<3; ++j) {
 			H_k_T(i,j) = R_CM_k(j,i);
diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h
index c2c02b38..5a2d7c0f 100644
--- a/ftnoir_tracker_aruco/trans_calib.h
+++ b/ftnoir_tracker_aruco/trans_calib.h
@@ -26,7 +26,7 @@ public:
     void reset();
 
     // update the current estimate
-    void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k);
+    void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k);
 
     // get the current estimate for t_MH
     cv::Vec3f get_estimate();