summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui11
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp70
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.h11
-rw-r--r--ftnoir_tracker_aruco/trans_calib.cpp44
-rw-r--r--ftnoir_tracker_aruco/trans_calib.h39
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