summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--tracker-aruco/CMakeLists.txt14
-rw-r--r--tracker-aruco/aruco-trackercontrols.ui13
-rw-r--r--tracker-aruco/ftnoir_tracker_aruco.cpp165
-rw-r--r--tracker-aruco/ftnoir_tracker_aruco.h56
-rw-r--r--tracker-aruco/trans_calib.cpp41
-rw-r--r--tracker-aruco/trans_calib.h39
6 files changed, 200 insertions, 128 deletions
diff --git a/tracker-aruco/CMakeLists.txt b/tracker-aruco/CMakeLists.txt
index c0b50bee..d68b8f59 100644
--- a/tracker-aruco/CMakeLists.txt
+++ b/tracker-aruco/CMakeLists.txt
@@ -1,7 +1,9 @@
-find_package(OpenCV 3.0 QUIET COMPONENTS ${opencv-modules})
-set(SDK_ARUCO_LIBPATH "" CACHE FILEPATH "Aruco paper marker tracker static library path")
-if(SDK_ARUCO_LIBPATH AND OpenCV_FOUND)
- opentrack_boilerplate(opentrack-tracker-aruco)
- target_link_libraries(opentrack-tracker-aruco opentrack-cv ${SDK_ARUCO_LIBPATH} ${OpenCV_LIBS})
- target_include_directories(opentrack-tracker-aruco SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
+find_package(OpenCV 3.0 QUIET)
+if(OpenCV_FOUND)
+ set(SDK_ARUCO_LIBPATH "" CACHE FILEPATH "Aruco paper marker tracker static library path")
+ if(SDK_ARUCO_LIBPATH)
+ opentrack_boilerplate(opentrack-tracker-aruco)
+ target_link_libraries(opentrack-tracker-aruco opentrack-cv ${SDK_ARUCO_LIBPATH} ${OpenCV_LIBS})
+ target_include_directories(opentrack-tracker-aruco SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
+ endif()
endif()
diff --git a/tracker-aruco/aruco-trackercontrols.ui b/tracker-aruco/aruco-trackercontrols.ui
index 1479b2e7..5f043098 100644
--- a/tracker-aruco/aruco-trackercontrols.ui
+++ b/tracker-aruco/aruco-trackercontrols.ui
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
- <class>dialog_aruco</class>
- <widget class="QWidget" name="dialog_aruco">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
<property name="windowModality">
<enum>Qt::NonModal</enum>
</property>
@@ -145,10 +145,10 @@
<locale language="English" country="UnitedStates"/>
</property>
<property name="minimum">
- <number>10</number>
+ <number>35</number>
</property>
<property name="maximum">
- <number>180</number>
+ <number>90</number>
</property>
</widget>
</item>
@@ -172,11 +172,6 @@
</item>
<item>
<property name="text">
- <string>320x200</string>
- </property>
- </item>
- <item>
- <property name="text">
<string>Default (not recommended!)</string>
</property>
</item>
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp
index 8edb8cf5..0df89fcb 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.cpp
+++ b/tracker-aruco/ftnoir_tracker_aruco.cpp
@@ -5,13 +5,13 @@
* copyright notice and this permission notice appear in all copies.
*/
+#include "cv/video-widget.hpp"
#include "ftnoir_tracker_aruco.h"
-#include "api/plugin-api.hpp"
-#include "compat/camera-names.hpp"
#include "cv/video-property-page.hpp"
+#include "compat/camera-names.hpp"
+#include "compat/sleep.hpp"
#include <opencv2/core.hpp>
-#include <opencv2/videoio.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
@@ -30,34 +30,35 @@ struct resolution_tuple
int height;
};
-static resolution_tuple resolution_choices[] =
+static constexpr const resolution_tuple resolution_choices[] =
{
{ 640, 480 },
{ 320, 240 },
- { 320, 200 },
{ 0, 0 }
};
-tracker_aruco::tracker_aruco() :
+constexpr const double aruco_tracker::RC;
+constexpr const double aruco_tracker::size_min;
+constexpr const double aruco_tracker::size_max;
+
+aruco_tracker::aruco_tracker() :
stop(false),
layout(nullptr),
videoWidget(nullptr),
+ fps(0),
obj_points(4),
- intrinsics(decltype(intrinsics)::eye()),
- dist_coeffs(decltype(dist_coeffs)::zeros()),
- rmat(decltype(rmat)::eye()),
+ intrinsics(cv::Matx33d::eye()),
+ rmat(cv::Matx33d::eye()),
roi_points(4),
- last_roi(65535, 65535, 0, 0),
- freq(cv::getTickFrequency()), // XXX change to timer.hpp
- cur_fps(0)
+ last_roi(65535, 65535, 0, 0)
{
// param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco.
- detector.setThresholdParams(5, -1);
+ detector.setThresholdParams(7, -1);
detector.setDesiredSpeed(3);
detector._thresMethod = aruco::MarkerDetector::FIXED_THRES;
}
-tracker_aruco::~tracker_aruco()
+aruco_tracker::~aruco_tracker()
{
stop = true;
wait();
@@ -65,10 +66,12 @@ tracker_aruco::~tracker_aruco()
delete videoWidget;
if(layout)
delete layout;
+ // fast start/stop causes breakage
+ portable::sleep(1000);
camera.release();
}
-void tracker_aruco::start_tracker(QFrame* videoframe)
+void aruco_tracker::start_tracker(QFrame* videoframe)
{
videoframe->show();
videoWidget = new cv_video_widget(videoframe);
@@ -85,9 +88,7 @@ void tracker_aruco::start_tracker(QFrame* videoframe)
layout = layout_;
}
-#define HT_PI M_PI
-
-void tracker_aruco::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
+void aruco_tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
{
QMutexLocker l(&mtx);
@@ -95,17 +96,14 @@ void tracker_aruco::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
t_ = t;
}
-bool tracker_aruco::detect_with_roi()
+bool aruco_tracker::detect_with_roi()
{
if (last_roi.width > 1 && last_roi.height > 1)
{
- float min = std::min(1.f, std::max(.01f, size_min * grayscale.cols / last_roi.width));
- float max = std::max(.01f, std::min(1.f, size_max * grayscale.cols / last_roi.width));
- detector.setMinMaxSize(min, max);
+ detector.setMinMaxSize(std::min(1., std::max(.01, size_min * grayscale.cols / last_roi.width)),
+ std::max(.01, std::min(1., size_max * grayscale.cols / last_roi.width)));
- cv::Mat grayscale_ = grayscale(last_roi);
-
- detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false);
+ detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false);
if (markers.size() == 1 && markers[0].size() == 4)
{
@@ -124,14 +122,14 @@ bool tracker_aruco::detect_with_roi()
return false;
}
-bool tracker_aruco::detect_without_roi()
+bool aruco_tracker::detect_without_roi()
{
detector.setMinMaxSize(size_min, size_max);
detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
return markers.size() == 1 && markers[0].size() == 4;
}
-bool tracker_aruco::open_camera()
+bool aruco_tracker::open_camera()
{
int rint = s.resolution;
if (rint < 0 || rint >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple)))
@@ -180,7 +178,7 @@ bool tracker_aruco::open_camera()
return true;
}
-void tracker_aruco::set_intrinsics()
+void aruco_tracker::set_intrinsics()
{
const int w = grayscale.cols, h = grayscale.rows;
const double diag_fov = static_cast<int>(s.fov) * M_PI / 180.;
@@ -195,16 +193,20 @@ void tracker_aruco::set_intrinsics()
intrinsics(1, 2) = grayscale.rows/2;
}
-void tracker_aruco::update_fps(double alpha)
+void aruco_tracker::update_fps()
{
- std::uint64_t time = std::uint64_t(cv::getTickCount());
- const double dt = std::max(0., (time - last_time) / freq);
- last_time = time;
+ const double dt = fps_timer.elapsed_seconds();
+ fps_timer.start();
+ const double alpha = dt/(dt + RC);
- cur_fps = cur_fps * alpha + (1-alpha) * (fabs(dt) < 1e-6 ? 0 : 1./dt);
+ if (dt > 1e-3)
+ {
+ fps *= 1 - alpha;
+ fps += alpha * (1./dt + .8);
+ }
}
-void tracker_aruco::draw_ar(bool ok)
+void aruco_tracker::draw_ar(bool ok)
{
if (ok)
{
@@ -213,13 +215,13 @@ void tracker_aruco::draw_ar(bool ok)
cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8);
}
- char buf[32];
- ::snprintf(buf, sizeof(buf)-1, "Hz: %d", (int)(unsigned short)cur_fps);
+ char buf[9];
+ ::snprintf(buf, sizeof(buf)-1, "Hz: %d", clamp(int(fps), 0, 9999));
buf[sizeof(buf)-1] = '\0';
cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1);
}
-void tracker_aruco::clamp_last_roi()
+void aruco_tracker::clamp_last_roi()
{
if (last_roi.x < 0)
last_roi.x = 0;
@@ -242,7 +244,7 @@ void tracker_aruco::clamp_last_roi()
last_roi.height -= last_roi.y;
}
-void tracker_aruco::set_points()
+void aruco_tracker::set_points()
{
using f = float;
const f hx = f(s.headpos_x), hy = f(s.headpos_y), hz = f(s.headpos_z);
@@ -257,19 +259,18 @@ void tracker_aruco::set_points()
obj_points[x4] = cv::Point3f(-size + hx, size + hy, 0 + hz);
}
-void tracker_aruco::draw_centroid()
+void aruco_tracker::draw_centroid()
{
repr2.clear();
static const std::vector<cv::Point3f> centroid { cv::Point3f(0, 0, 0) };
- cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
+ cv::projectPoints(centroid, rvec, tvec, intrinsics, cv::noArray(), repr2);
- auto s = cv::Scalar(255, 0, 255);
- cv::circle(frame, repr2[0], 4, s, -1);
+ cv::circle(frame, repr2[0], 4, cv::Scalar(255, 0, 255), -1);
}
-void tracker_aruco::set_last_roi()
+void aruco_tracker::set_last_roi()
{
roi_projection.clear();
@@ -281,12 +282,12 @@ void tracker_aruco::set_last_roi()
roi_points[i] = pt * c_search_window;
}
- cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
+ cv::projectPoints(roi_points, rvec, tvec, intrinsics, cv::noArray(), roi_projection);
set_roi_from_projection();
}
-void tracker_aruco::set_rmat()
+void aruco_tracker::set_rmat()
{
cv::Rodrigues(rvec, rmat);
@@ -305,7 +306,7 @@ void tracker_aruco::set_rmat()
t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]);
}
-void tracker_aruco::set_roi_from_projection()
+void aruco_tracker::set_roi_from_projection()
{
last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0);
@@ -328,7 +329,7 @@ void tracker_aruco::set_roi_from_projection()
clamp_last_roi();
}
-void tracker_aruco::run()
+void aruco_tracker::run()
{
cv::setNumThreads(0);
@@ -340,7 +341,7 @@ void tracker_aruco::run()
if (!open_camera())
return;
- last_time = std::uint64_t(cv::getTickCount());
+ fps_timer.start();
while (!stop)
{
@@ -357,7 +358,7 @@ void tracker_aruco::run()
set_intrinsics();
- update_fps(alpha_);
+ update_fps();
markers.clear();
@@ -367,10 +368,7 @@ void tracker_aruco::run()
{
set_points();
- if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_DLS))
- goto fail;
-
- if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, true, cv::SOLVEPNP_ITERATIVE))
+ if (!cv::solvePnP(obj_points, markers[0], intrinsics, cv::noArray(), rvec, tvec, false, cv::SOLVEPNP_ITERATIVE))
goto fail;
set_last_roi();
@@ -389,19 +387,19 @@ fail:
}
}
-void tracker_aruco::data(double *data)
+void aruco_tracker::data(double *data)
{
QMutexLocker lck(&mtx);
data[Yaw] = pose[Yaw];
data[Pitch] = pose[Pitch];
data[Roll] = pose[Roll];
- data[TX] = pose[TX] * .5;
- data[TY] = pose[TY] * .5;
+ data[TX] = pose[TX];
+ data[TY] = pose[TY];
data[TZ] = pose[TZ];
}
-dialog_aruco::dialog_aruco()
+aruco_dialog::aruco_dialog()
{
tracker = nullptr;
calib_timer.setInterval(250);
@@ -419,34 +417,15 @@ dialog_aruco::dialog_aruco()
connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
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()));
- connect(ui.cameraName, &QComboBox::currentTextChanged, this, &dialog_aruco::set_camera_settings_available);
- set_camera_settings_available(ui.cameraName->currentText());
- connect(ui.camera_settings, &QPushButton::clicked, this, &dialog_aruco::show_camera_settings);
-}
+ connect(ui.camera_settings, SIGNAL(clicked()), this, SLOT(camera_settings()));
+ connect(&s.camera_name, SIGNAL(valueChanged(const QString&)), this, SLOT(update_camera_settings_state(const QString&)));
-void dialog_aruco::set_camera_settings_available(const QString& camera_name)
-{
- const bool avail = video_property_page::should_show_dialog(camera_name);
- ui.camera_settings->setEnabled(avail);
+ update_camera_settings_state(s.camera_name);
}
-void dialog_aruco::show_camera_settings()
-{
- const int idx = ui.cameraName->currentIndex();
- if (tracker)
- {
- cv::VideoCapture& cap = tracker->camera;
- if (cap.isOpened())
- video_property_page::show_from_capture(cap, idx);
- }
- else
- video_property_page::show(idx);
-}
-
-void dialog_aruco::toggleCalibrate()
+void aruco_dialog::toggleCalibrate()
{
if (!calib_timer.isActive())
{
@@ -467,13 +446,13 @@ void dialog_aruco::toggleCalibrate()
}
}
-void dialog_aruco::cleanupCalib()
+void aruco_dialog::cleanupCalib()
{
if (calib_timer.isActive())
calib_timer.stop();
}
-void dialog_aruco::update_tracker_calibration()
+void aruco_dialog::update_tracker_calibration()
{
if (calib_timer.isActive() && tracker)
{
@@ -484,15 +463,31 @@ void dialog_aruco::update_tracker_calibration()
}
}
-void dialog_aruco::doOK()
+void aruco_dialog::doOK()
{
s.b->save();
close();
}
-void dialog_aruco::doCancel()
+void aruco_dialog::doCancel()
{
close();
}
-OPENTRACK_DECLARE_TRACKER(tracker_aruco, dialog_aruco, aruco_metadata)
+void aruco_dialog::camera_settings()
+{
+ if (tracker)
+ {
+ QMutexLocker l(&tracker->camera_mtx);
+ video_property_page::show_from_capture(tracker->camera, camera_name_to_index(s.camera_name));
+ }
+ else
+ video_property_page::show(camera_name_to_index(s.camera_name));
+}
+
+void aruco_dialog::update_camera_settings_state(const QString& name)
+{
+ ui.camera_settings->setEnabled(video_property_page::should_show_dialog(name));
+}
+
+OPENTRACK_DECLARE_TRACKER(aruco_tracker, aruco_dialog, aruco_metadata)
diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h
index ccb1ad21..54940d28 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.h
+++ b/tracker-aruco/ftnoir_tracker_aruco.h
@@ -8,14 +8,13 @@
#pragma once
#include "ui_aruco-trackercontrols.h"
+#include "options/options.hpp"
+#include "trans_calib.h"
#include "api/plugin-api.hpp"
-#include "include/markerdetector.h"
-
#include "cv/video-widget.hpp"
-#include "cv/translation-calibrator.hpp"
+#include "compat/timer.hpp"
-#include <opencv2/core.hpp>
-#include <opencv2/videoio.hpp>
+#include "include/markerdetector.h"
#include <QObject>
#include <QThread>
@@ -26,7 +25,9 @@
#include <cinttypes>
-#include "options/options.hpp"
+#include <opencv2/core.hpp>
+#include <opencv2/videoio.hpp>
+
using namespace options;
struct settings : opts {
@@ -46,16 +47,16 @@ struct settings : opts {
{}
};
-class dialog_aruco;
+class aruco_dialog;
-class tracker_aruco : protected QThread, public ITracker
+class aruco_tracker : protected QThread, public ITracker
{
Q_OBJECT
- friend class dialog_aruco;
+ friend class aruco_dialog;
static constexpr float c_search_window = 1.3f;
public:
- tracker_aruco();
- ~tracker_aruco() override;
+ aruco_tracker();
+ ~aruco_tracker() override;
void start_tracker(QFrame* frame) override;
void data(double *data) override;
void run() override;
@@ -65,7 +66,7 @@ private:
bool detect_without_roi();
bool open_camera();
void set_intrinsics();
- void update_fps(double dt);
+ void update_fps();
void draw_ar(bool ok);
void clamp_last_roi();
void set_points();
@@ -77,16 +78,14 @@ private:
cv::VideoCapture camera;
QMutex camera_mtx;
QMutex mtx;
- volatile bool stop;
QHBoxLayout* layout;
cv_video_widget* videoWidget;
settings s;
- double pose[6];
+ double pose[6], fps;
cv::Mat frame, grayscale, color;
cv::Matx33d r;
std::vector<cv::Point3f> obj_points;
cv::Matx33d intrinsics;
- cv::Matx14f dist_coeffs;
aruco::MarkerDetector detector;
std::vector<aruco::Marker> markers;
cv::Vec3d t;
@@ -97,25 +96,26 @@ private:
cv::Vec3d euler;
std::vector<cv::Point3f> roi_points;
cv::Rect last_roi;
- double freq, cur_fps;
- std::uint64_t last_time;
+ Timer fps_timer;
+
+ volatile bool stop;
- static constexpr float size_min = 0.05f;
- static constexpr float size_max = 0.3f;
+ static constexpr const double size_min = 0.05f;
+ static constexpr const double size_max = 0.3f;
- static constexpr double alpha_ = .95;
+ static constexpr const double RC = .25;
};
-class dialog_aruco : public ITrackerDialog
+class aruco_dialog : public ITrackerDialog
{
Q_OBJECT
public:
- dialog_aruco();
- void register_tracker(ITracker * x) override { tracker = static_cast<tracker_aruco*>(x); }
+ aruco_dialog();
+ void register_tracker(ITracker * x) override { tracker = static_cast<aruco_tracker*>(x); }
void unregister_tracker() override { tracker = nullptr; }
private:
- Ui::dialog_aruco ui;
- tracker_aruco* tracker;
+ Ui::Form ui;
+ aruco_tracker* tracker;
settings s;
TranslationCalibrator calibrator;
QTimer calib_timer;
@@ -125,12 +125,12 @@ private slots:
void toggleCalibrate();
void cleanupCalib();
void update_tracker_calibration();
- void set_camera_settings_available(const QString& camera_name);
- void show_camera_settings();
+ void camera_settings();
+ void update_camera_settings_state(const QString& name);
};
class aruco_metadata : public Metadata
{
- QString name() { return QString(QCoreApplication::translate("aruco_metadata", "aruco -- paper marker tracker")); }
+ QString name() { return QString("aruco -- paper marker tracker"); }
QIcon icon() { return QIcon(":/images/aruco.png"); }
};
diff --git a/tracker-aruco/trans_calib.cpp b/tracker-aruco/trans_calib.cpp
new file mode 100644
index 00000000..b5148efd
--- /dev/null
+++ b/tracker-aruco/trans_calib.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 "trans_calib.h"
+
+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/tracker-aruco/trans_calib.h b/tracker-aruco/trans_calib.h
new file mode 100644
index 00000000..3d1e56fa
--- /dev/null
+++ b/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/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)
+};
+
+#endif //TRANSCALIB_H
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-21 17:47:25 +0000