summaryrefslogtreecommitdiffhomepage
path: root/tracker-aruco/ftnoir_tracker_aruco.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-aruco/ftnoir_tracker_aruco.cpp')
-rw-r--r--tracker-aruco/ftnoir_tracker_aruco.cpp165
1 files changed, 80 insertions, 85 deletions
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)