From 4dc41d3d8642d90d52b667dd05b4f99735f83a37 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 11 Jul 2014 17:23:20 +0200 Subject: integrate PT axis of rotation estimation into AR --- ftnoir_tracker_aruco/aruco-trackercontrols.ui | 11 ++++- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 70 ++++++++++++++++++++++++--- ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 11 +++++ ftnoir_tracker_aruco/trans_calib.cpp | 44 +++++++++++++++++ ftnoir_tracker_aruco/trans_calib.h | 39 +++++++++++++++ 5 files changed, 166 insertions(+), 9 deletions(-) create mode 100644 ftnoir_tracker_aruco/trans_calib.cpp create mode 100644 ftnoir_tracker_aruco/trans_calib.h 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 @@ 0 0 - 615 - 326 + 636 + 346 @@ -110,6 +110,13 @@ + + + + Calibrate + + + 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(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 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 #include #include +#include #include #include #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 H_k_T = Matx::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 + +//----------------------------------------------------------------------------- +// 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 -- cgit v1.2.3