integrate PT axis of rotation estimation into AR

5 files changed, 166 insertions, 9 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 1d5a4241..be237e3c 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>615</width>
-    <height>326</height>
+    <width>636</width>
+    <height>346</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -110,6 +110,13 @@
         </property>
        </widget>
       </item>
+      <item row="4" column="1">
+       <widget class="QPushButton" name="btn_calibrate">
+        <property name="text">
+         <string>Calibrate</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index ae7ca0b5..756278cd 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -137,12 +137,20 @@ void Tracker::StartTracker(QFrame* videoframe)
 
 #define HT_PI 3.1415926535
 
+void Tracker::getRT(cv::Matx33f& r_, cv::Vec3f& t_)
+{
+    QMutexLocker l(&mtx);
+
+    r_ = r;
+    t_ = t;
+}
+
 void Tracker::run()
 {
-    int res = s.resolution;
-    if (res < 0 || res >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple)))
-        res = 0;
-    resolution_tuple r = resolution_choices[res];
+    int rint = s.resolution;
+    if (rint < 0 || rint >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple)))
+        rint = 0;
+    resolution_tuple res = resolution_choices[rint];
     int fps;
     switch (static_cast<int>(s.force_fps))
     {
@@ -164,10 +172,10 @@ void Tracker::run()
         break;
     }
     camera = cv::VideoCapture(s.camera_index);
-    if (r.width)
+    if (res.width)
     {
-        camera.set(CV_CAP_PROP_FRAME_WIDTH, r.width);
-        camera.set(CV_CAP_PROP_FRAME_HEIGHT, r.height);
+        camera.set(CV_CAP_PROP_FRAME_WIDTH, res.width);
+        camera.set(CV_CAP_PROP_FRAME_HEIGHT, res.height);
     }
     if (fps)
         camera.set(CV_CAP_PROP_FPS, fps);
@@ -340,6 +348,9 @@ void Tracker::run()
                 pose[Yaw] = euler[1];
                 pose[Pitch] = -euler[0];
                 pose[Roll] = euler[2];
+
+                rotation_matrix.convertTo(r, CV_32FC1);
+                tvec.convertTo(t, CV_32FC1);
             }
 
             std::vector<cv::Point2f> repr2;
@@ -446,6 +457,8 @@ extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDial
 TrackerControls::TrackerControls()
 {
     tracker = nullptr;
+    calibrating = false;
+    calib_timer.setInterval(100);
 	ui.setupUi(this);
     setAttribute(Qt::WA_NativeWindow, true);
     tie_setting(s.camera_index, ui.cameraName);
@@ -466,6 +479,49 @@ TrackerControls::TrackerControls()
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
     ui.cameraName->addItems(get_camera_names());
+
+    connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate()));
+    connect(this, SIGNAL(destroyed()), this, SLOT(cleanupCalib()));
+    connect(&calib_timer, SIGNAL(timeout()), this, SLOT(update_tracker_calibration()));
+}
+
+void TrackerControls::toggleCalibrate()
+{
+    if (!calibrating)
+    {
+        calibrator.reset();
+        calib_timer.start();
+    } else {
+        cleanupCalib();
+    }
+    calibrating = !calibrating;
+}
+
+void TrackerControls::cleanupCalib()
+{
+    if (calibrating)
+    {
+        calib_timer.stop();
+        auto pos = calibrator.get_estimate() * .1;
+        s.headpos_x = pos(0);
+        s.headpos_y = pos(1);
+        s.headpos_z = pos(2);
+    }
+}
+
+void TrackerControls::update_tracker_calibration()
+{
+    if (calibrating && tracker)
+    {
+        cv::Matx33f r;
+        cv::Vec3f t;
+        tracker->getRT(r, t);
+        calibrator.update(r, t);
+        auto pos = calibrator.get_estimate() * .1;
+        s.headpos_x = pos(0);
+        s.headpos_y = pos(1);
+        s.headpos_z = pos(2);
+    }
 }
 
 void TrackerControls::doOK()
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 4cab84b5..f1fcc326 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -16,9 +16,11 @@
 #include <QMutex>
 #include <QHBoxLayout>
 #include <QDialog>
+#include <QTimer>
 #include <opencv2/opencv.hpp>
 #include <opencv/highgui.h>
 #include "facetracknoir/options.h"
+#include "ftnoir_tracker_aruco/trans_calib.h"
 using namespace options;
 
 struct settings {
@@ -58,6 +60,7 @@ public:
     void GetHeadPoseData(double *data);
     void run();
     void reload() { s.b->reload(); }
+    void getRT(cv::Matx33f& r, cv::Vec3f& t);
 private:
     QMutex mtx;
     volatile bool stop;
@@ -67,6 +70,8 @@ private:
     double pose[6];
     cv::Mat frame;
     cv::VideoCapture camera;
+    cv::Matx33f r;
+    cv::Vec3f t;
 };
 
 // Widget that has controls for FTNoIR protocol client-settings.
@@ -85,9 +90,15 @@ private:
 	Ui::Form ui;
     Tracker* tracker;
     settings s;
+    TranslationCalibrator calibrator;
+    bool calibrating;
+    QTimer calib_timer;
 private slots:
 	void doOK();
 	void doCancel();
+    void toggleCalibrate();
+    void cleanupCalib();
+    void update_tracker_calibration();
 };
 
 #endif
diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp
new file mode 100644
index 00000000..dd18399a
--- /dev/null
+++ b/ftnoir_tracker_aruco/trans_calib.cpp
@@ -0,0 +1,44 @@
+/* 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 "trans_calib.h"

+

+using namespace cv;

+

+//-----------------------------------------------------------------------------

+TranslationCalibrator::TranslationCalibrator()

+{

+	reset();

+}

+

+void TranslationCalibrator::reset()

+{

+	P = Matx66f::zeros();

+	y = Vec6f(0,0,0, 0,0,0);

+}

+

+void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k)

+{

+	Matx<float, 6,3> H_k_T = Matx<float, 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;

+}

+

+Vec3f TranslationCalibrator::get_estimate()

+{

+	Vec6f x = P.inv() * y;

+    return Vec3f(x[0], x[1], x[2]);

+}

diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h
new file mode 100644
index 00000000..f2521690
--- /dev/null
+++ b/ftnoir_tracker_aruco/trans_calib.h
@@ -0,0 +1,39 @@
+/* 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.

+ */

+

+#ifndef TRANSCALIB_H

+#define TRANSCALIB_H

+

+#include <opencv2/opencv.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::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k);

+

+	// get the current estimate for t_MH

+	cv::Vec3f get_estimate();

+

+protected:

+	cv::Matx66f P;	// normalized precision matrix = inverse covariance

+	cv::Vec6f y;	// P*(-t_MH, t_CH)

+};

+

+#endif //TRANSCALIB_H
\ No newline at end of file