cv: move calibrator and video widget to cv module

Adjust usages in PT and Aruco trackers.

5 files changed, 187 insertions, 28 deletions

diff --git a/cv/export.hpp b/cv/export.hpp
deleted file mode 100644
index 6636e56b..00000000
--- a/cv/export.hpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-#ifdef BUILD_cv
-#   ifdef _WIN32
-#       define OPENTRACK_CV_LINKAGE __declspec(dllexport)
-#   else
-#       define OPENTRACK_CV_LINKAGE
-#   endif
-
-#   ifndef _MSC_VER
-#       define OPENTRACK_CV_EXPORT __attribute__ ((visibility ("default"))) OPENTRACK_CV_LINKAGE
-#   else
-#       define OPENTRACK_CV_EXPORT OPENTRACK_CV_LINKAGE
-#   endif
-
-#else
-    #ifdef _WIN32
-    #    define OPENTRACK_CV_LINKAGE __declspec(dllimport)
-    #else
-    #    define OPENTRACK_CV_LINKAGE
-    #endif
-
-    #ifndef _MSC_VER
-    #    define OPENTRACK_CV_EXPORT __attribute__ ((visibility ("default"))) OPENTRACK_CV_LINKAGE
-    #else
-    #    define OPENTRACK_CV_EXPORT OPENTRACK_CV_LINKAGE
-    #endif
-#endif
diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
new file mode 100644
index 00000000..1287b408
--- /dev/null
+++ b/cv/translation-calibrator.cpp
@@ -0,0 +1,41 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "translation-calibrator.hpp"
+
+TranslationCalibrator::TranslationCalibrator()
+{
+    reset();
+}
+
+void TranslationCalibrator::reset()
+{
+    P = cv::Matx66f::zeros();
+    y = cv::Vec6f(0,0,0, 0,0,0);
+}
+
+void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k)
+{
+    cv::Matx<double, 6,3> H_k_T = cv::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);
+        }
+    }
+    for (int i=0; i<3; ++i)
+    {
+        H_k_T(3+i,i) = 1.0;
+    }
+    P += H_k_T * H_k_T.t();
+    y += H_k_T * t_CM_k;
+}
+
+cv::Vec3f TranslationCalibrator::get_estimate()
+{
+    cv::Vec6f x = P.inv() * y;
+    return cv::Vec3f(x[0], x[1], x[2]);
+}
diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp
new file mode 100644
index 00000000..cfde0051
--- /dev/null
+++ b/cv/translation-calibrator.hpp
@@ -0,0 +1,36 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+//-----------------------------------------------------------------------------
+// Calibrates the translation from head to model = t_MH
+// by recursive least squares /
+// kalman filter in information form with identity noise covariance
+// measurement equation when head position = t_CH is fixed:
+// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k
+
+class TranslationCalibrator
+{
+public:
+    TranslationCalibrator();
+
+    // reset the calibration process
+    void reset();
+
+    // update the current estimate
+    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();
+
+private:
+    cv::Matx66f P;  // normalized precision matrix = inverse covariance
+    cv::Vec6f y;    // P*(-t_MH, t_CH)
+};
diff --git a/cv/video-widget.cpp b/cv/video-widget.cpp
new file mode 100644
index 00000000..b514dc8f
--- /dev/null
+++ b/cv/video-widget.cpp
@@ -0,0 +1,71 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-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
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "video-widget.hpp"
+#include <opencv2/imgproc.hpp>
+
+#include "opentrack/is-window-visible.hpp"
+
+cv_video_widget::cv_video_widget(QWidget* parent) :
+    QWidget(parent),
+    freshp(false),
+    visible(true)
+{
+    connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
+    timer.start(50);
+}
+
+void cv_video_widget::update_image(const cv::Mat& frame)
+{
+    QMutexLocker foo(&mtx);
+
+    if (!freshp)
+    {
+        if (_frame.cols != frame.cols || _frame.rows != frame.rows)
+        {
+            _frame = cv::Mat(frame.rows, frame.cols, CV_8U);
+            _frame2 = cv::Mat(frame.rows, frame.cols, CV_8U);
+        }
+        frame.copyTo(_frame);
+        freshp = true;
+    }
+}
+
+void cv_video_widget::paintEvent(QPaintEvent*)
+{
+    QMutexLocker foo(&mtx);
+    QPainter painter(this);
+    painter.drawImage(rect(), texture);
+}
+
+void cv_video_widget::update_and_repaint()
+{
+    QMutexLocker l(&mtx);
+
+    if (window_check_timer.elapsed_ms() > 250)
+    {
+        visible = is_window_visible(this);
+        window_check_timer.start();
+    }
+
+    if (visible)
+    {
+        if (_frame.empty() || !freshp)
+            return;
+        cv::cvtColor(_frame, _frame2, cv::COLOR_RGB2BGR);
+
+        if (_frame3.cols != width() || _frame3.rows != height())
+            _frame3 = cv::Mat(height(), width(), CV_8U);
+
+        cv::resize(_frame2, _frame3, cv::Size(width(), height()), 0, 0, cv::INTER_NEAREST);
+
+        texture = QImage((const unsigned char*) _frame3.data, _frame3.cols, _frame3.rows, QImage::Format_RGB888);
+        freshp = false;
+        update();
+    }
+}
diff --git a/cv/video-widget.hpp b/cv/video-widget.hpp
new file mode 100644
index 00000000..2d5d673f
--- /dev/null
+++ b/cv/video-widget.hpp
@@ -0,0 +1,39 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ * Copyright (c) 2014-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
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#pragma once
+
+#include "opentrack-compat/timer.hpp"
+#include <opencv2/core/core.hpp>
+#include <memory>
+#include <QObject>
+#include <QWidget>
+#include <QPainter>
+#include <QPaintEvent>
+#include <QTimer>
+#include <QMutex>
+#include <QMutexLocker>
+#include <QDebug>
+
+class cv_video_widget final : public QWidget
+{
+    Q_OBJECT
+public:
+    cv_video_widget(QWidget *parent);
+    void update_image(const cv::Mat &frame);
+protected slots:
+    void paintEvent(QPaintEvent*) override;
+    void update_and_repaint();
+private:
+    QMutex mtx;
+    QImage texture;
+    QTimer timer;
+    Timer window_check_timer;
+    cv::Mat _frame, _frame2, _frame3;
+    bool freshp, visible;
+};