diff options
| -rw-r--r-- | ftnoir_tracker_aruco/aruco-trackercontrols.ui | 11 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 70 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 11 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.cpp | 44 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.h | 39 |
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 |