diff options
Diffstat (limited to 'tracker-aruco/ftnoir_tracker_aruco.cpp')
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.cpp | 319 |
1 files changed, 190 insertions, 129 deletions
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index edb6ffcc..5130a889 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -5,11 +5,14 @@ * copyright notice and this permission notice appear in all copies. */ -#include "cv/video-widget.hpp" #include "ftnoir_tracker_aruco.h" -#include "cv/video-property-page.hpp" -#include "compat/camera-names.hpp" #include "compat/sleep.hpp" +#include "compat/math-imports.hpp" +#include "cv/init.hpp" + +#ifdef _MSC_VER +# pragma warning(disable : 4702) +#endif #include <opencv2/core.hpp> #include <opencv2/imgproc.hpp> @@ -23,61 +26,69 @@ #include <QDebug> #include <vector> +#include <tuple> #include <cstdio> #include <cmath> #include <algorithm> #include <iterator> -constexpr double aruco_tracker::timeout; -constexpr double aruco_tracker::timeout_backoff_c; -constexpr const int aruco_tracker::adaptive_sizes[]; - -constexpr const aruco_tracker::resolution_tuple aruco_tracker::resolution_choices[]; - -constexpr const double aruco_tracker::RC; -constexpr const float aruco_tracker::size_min; -constexpr const float aruco_tracker::size_max; - -#ifdef DEBUG_UNSHARP_MASKING -constexpr double aruco_tracker::gauss_kernel_size; +static const int adaptive_sizes[] = +{ +#if defined USE_EXPERIMENTAL_CANNY + 10, + 30, + 80, +#else + 5, + 7, + 9, + 11, #endif +}; -aruco_tracker::aruco_tracker() : - pose{0,0,0, 0,0,0}, - fps(0), - no_detection_timeout(0), - obj_points(4), - intrinsics(cv::Matx33d::eye()), - rmat(cv::Matx33d::eye()), - roi_points(4), - last_roi(65535, 65535, 0, 0), - adaptive_size_pos(0), - stop(false), - use_otsu(false) +struct resolution_tuple +{ + int width; + int height; +}; + +static const resolution_tuple resolution_choices[] = +{ + { 640, 480 }, + { 320, 240 }, + { 1280, 720 }, + { 1920, 1080 }, + { 0, 0 } +}; + +aruco_tracker::aruco_tracker() { + opencv_init(); + // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco. set_detector_params(); } aruco_tracker::~aruco_tracker() { - stop = true; + requestInterruption(); wait(); // fast start/stop causes breakage portable::sleep(1000); - camera.release(); } -void aruco_tracker::start_tracker(QFrame* videoframe) +module_status aruco_tracker::start_tracker(QFrame* videoframe) { videoframe->show(); - videoWidget = qptr<cv_video_widget>(videoframe); - layout = qptr<QHBoxLayout>(); + videoWidget = std::make_unique<cv_video_widget>(videoframe); + layout = std::make_unique<QHBoxLayout>(); layout->setContentsMargins(0, 0, 0, 0); - layout->addWidget(videoWidget.data()); - videoframe->setLayout(layout.data()); + layout->addWidget(&*videoWidget); + videoframe->setLayout(&*layout); videoWidget->show(); start(); + + return status_ok(); } void aruco_tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_) @@ -92,11 +103,12 @@ bool aruco_tracker::detect_with_roi() { if (last_roi.width > 1 && last_roi.height > 1) { - detector.setMinMaxSize(clamp(size_min * grayscale.cols / last_roi.width, .01f, 1.f), - clamp(size_max * grayscale.cols / last_roi.width, .01f, 1.f)); + detector.setMinMaxSize(std::clamp(size_min * grayscale.cols / last_roi.width, .01f, 1.f), + std::clamp(size_max * grayscale.cols / last_roi.width, .01f, 1.f)); detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false); + // XXX TODO maybe cache the first-present marker id and force it's the same one? -sh 20180517 if (markers.size() == 1 && markers[0].size() == 4) { auto& m = markers[0]; @@ -121,48 +133,56 @@ bool aruco_tracker::detect_without_roi() return markers.size() == 1 && markers[0].size() == 4; } -bool aruco_tracker::open_camera() +static int enum_to_fps(int value) { - 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)) + + switch (value) { - default: - case 0: - fps = 0; - break; - case 1: - fps = 30; - break; - case 2: - fps = 60; - break; - case 3: - fps = 75; - break; - case 4: - fps = 125; - break; - case 5: - fps = 200; - break; + default: eval_once(qDebug() << "aruco: invalid fps enum value"); + [[fallthrough]]; + case fps_default: fps = 0; break; + case fps_30: fps = 30; break; + case fps_60: fps = 60; break; + case fps_75: fps = 75; break; + case fps_125: fps = 125; break; + case fps_200: fps = 200; break; + case fps_50: fps = 50; break; + case fps_100: fps = 100; break; + case fps_120: fps = 120; break; + case fps_300: fps = 300; break; + case fps_250: fps = 250; break; } + return fps; +} + +bool aruco_tracker::open_camera() +{ + int rint = std::clamp(*s.resolution, 0, (int)std::size(resolution_choices)-1); + resolution_tuple res = resolution_choices[rint]; + int fps = enum_to_fps(s.force_fps); + QMutexLocker l(&camera_mtx); - camera = cv::VideoCapture(camera_name_to_index(s.camera_name)); + camera = video::make_camera(s.camera_name); + + if (!camera) + return false; + + video::impl::camera::info args {}; + if (res.width) { - camera.set(cv::CAP_PROP_FRAME_WIDTH, res.width); - camera.set(cv::CAP_PROP_FRAME_HEIGHT, res.height); + args.width = res.width; + args.height = res.height; } if (fps) - camera.set(cv::CAP_PROP_FPS, fps); + args.fps = fps; + + args.use_mjpeg = s.use_mjpeg; - if (!camera.isOpened()) + if (!camera->start(args)) { qDebug() << "aruco tracker: can't open camera"; return false; @@ -173,16 +193,16 @@ bool aruco_tracker::open_camera() 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.; + const double diag_fov = s.fov * M_PI / 180.; const double fov_w = 2.*atan(tan(diag_fov/2.)/sqrt(1. + h/(double)w * h/(double)w)); const double fov_h = 2.*atan(tan(diag_fov/2.)/sqrt(1. + w/(double)h * w/(double)h)); const double focal_length_w = .5 * w / tan(.5 * fov_w); const double focal_length_h = .5 * h / tan(.5 * fov_h); intrinsics(0, 0) = focal_length_w; - intrinsics(0, 2) = grayscale.cols/2; + intrinsics(0, 2) = grayscale.cols/2.; intrinsics(1, 1) = focal_length_h; - intrinsics(1, 2) = grayscale.rows/2; + intrinsics(1, 2) = grayscale.rows/2.; } void aruco_tracker::update_fps() @@ -208,8 +228,7 @@ void aruco_tracker::draw_ar(bool ok) } char buf[9]; - ::snprintf(buf, sizeof(buf)-1, "Hz: %d", clamp(int(fps), 0, 9999)); - buf[sizeof(buf)-1] = '\0'; + ::snprintf(buf, sizeof(buf), "Hz: %d", std::clamp(int(fps), 0, 9999)); cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1); } @@ -218,37 +237,19 @@ void aruco_tracker::clamp_last_roi() last_roi &= cv::Rect(0, 0, color.cols, color.rows); } -cv::Point3f aruco_tracker::rotate_model(float x, float y, settings::rot mode) -{ - cv::Point3f pt(x, y, 0); - - if (mode) - { - using std::cos; - using std::sin; - - const float theta = int(mode) * 90/4. * M_PI/180; - pt.x = x * cos(theta) - y * sin(theta); - pt.y = y * cos(theta) + x * sin(theta); - } - return pt; -} - 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); - static constexpr float size = 40; + constexpr float size = 40; const int x1=1, x2=2, x3=3, x4=0; - settings::rot mode = s.model_rotation; - - obj_points[x1] = rotate_model(-size, -size, mode); - obj_points[x2] = rotate_model(size, -size, mode); - obj_points[x3] = rotate_model(size, size, mode); - obj_points[x4] = rotate_model(-size, size, mode); + obj_points[x1] = { -size, -size, 0 }; + obj_points[x2] = { size, -size, 0 }; + obj_points[x3] = { size, size, 0 }; + obj_points[x4] = { -size, size, 0 }; for (unsigned i = 0; i < 4; i++) obj_points[i] += cv::Point3f(hx, hy, hz); @@ -262,7 +263,7 @@ void aruco_tracker::draw_centroid() cv::projectPoints(centroid, rvec, tvec, intrinsics, cv::noArray(), repr2); - cv::circle(frame, repr2[0], 4, cv::Scalar(255, 0, 255), -1); + cv::circle(frame, repr2[0], 4, {255, 0, 255}, -1); } void aruco_tracker::set_last_roi() @@ -327,61 +328,85 @@ void aruco_tracker::set_roi_from_projection() void aruco_tracker::set_detector_params() { detector.setDesiredSpeed(3); - detector.setThresholdParams(adaptive_sizes[adaptive_size_pos], adaptive_thres); +#if !defined USE_EXPERIMENTAL_CANNY if (use_otsu) detector._thresMethod = aruco::MarkerDetector::FIXED_THRES; else detector._thresMethod = aruco::MarkerDetector::ADPT_THRES; + + detector.setThresholdParams(adaptive_sizes[adaptive_size_pos], adaptive_thres); +#else + detector._thresMethod = aruco::MarkerDetector::CANNY; + int value = adaptive_sizes[adaptive_size_pos]; + detector.setThresholdParams(value, value * 3); +#endif } void aruco_tracker::cycle_detection_params() { +#if !defined USE_EXPERIMENTAL_CANNY if (!use_otsu) use_otsu = true; else +#endif { use_otsu = false; adaptive_size_pos++; - adaptive_size_pos %= sizeof(adaptive_sizes)/sizeof(*adaptive_sizes); + adaptive_size_pos %= std::size(adaptive_sizes); } set_detector_params(); qDebug() << "aruco: switched thresholding params" +#if !defined USE_EXPERIMENTAL_CANNY << "otsu:" << use_otsu +#endif << "size:" << adaptive_sizes[adaptive_size_pos]; } void aruco_tracker::run() { - cv::setNumThreads(0); - - using std::fabs; - using std::atan; - using std::tan; - using std::sqrt; - if (!open_camera()) return; fps_timer.start(); last_detection_timer.start(); - while (!stop) + while (!isInterruptionRequested()) { { QMutexLocker l(&camera_mtx); - if (!camera.read(color)) + auto [ img, res ] = camera->get_frame(); + + if (!res) + { + camera_mtx.unlock(); + portable::sleep(100); continue; - } + } - cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); + color = cv::Mat(img.height, img.width, CV_8UC(img.channels), (void*)img.data, img.stride); + + switch (img.channels) + { + case 1: + grayscale.create(img.height, img.width, CV_8UC1); + color.copyTo(grayscale); + break; + case 3: + cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY); + break; + default: + qDebug() << "aruco: can't handle" << img.channels << "color channels"; + return; + } + } #ifdef DEBUG_UNSHARP_MASKING { - static constexpr double strength = double(DEBUG_UNSHARP_MASKING); + constexpr double strength = double(DEBUG_UNSHARP_MASKING); cv::GaussianBlur(grayscale, blurred, cv::Size(0, 0), gauss_kernel_size); cv::addWeighted(grayscale, 1 + strength, blurred, -strength, 0, grayscale); cv::imshow("capture", grayscale); @@ -410,7 +435,7 @@ void aruco_tracker::run() const double dt = last_detection_timer.elapsed_seconds(); last_detection_timer.start(); no_detection_timeout -= dt * timeout_backoff_c; - no_detection_timeout = std::max(0., no_detection_timeout); + no_detection_timeout = std::fmax(0., no_detection_timeout); } set_last_roi(); @@ -452,26 +477,59 @@ void aruco_tracker::data(double *data) data[TZ] = pose[TZ]; } +void aruco_dialog::make_fps_combobox() +{ + std::vector<std::tuple<int, int>> resolutions; + resolutions.reserve(fps_MAX); + + for (int k = 0; k < fps_MAX; k++) + { + int hz = enum_to_fps(k); + resolutions.emplace_back(k, hz); + } + + std::sort(resolutions.begin(), resolutions.end(), [](const auto& a, const auto& b) { + auto [idx1, hz1] = a; + auto [idx2, hz2] = b; + + return hz1 < hz2; + }); + + for (auto [idx, hz] : resolutions) + { + QString name; + + if (hz == 0) + name = tr("Default"); + else + name = QString::number(hz); + + ui.cameraFPS->addItem(name, idx); + } +} + aruco_dialog::aruco_dialog() : - calibrator(1, 0, 2) + calibrator(1, 0) { + ui.setupUi(this); + //setAttribute(Qt::WA_NativeWindow, true); + + make_fps_combobox(); + tie_setting(s.force_fps, ui.cameraFPS); + tracker = nullptr; calib_timer.setInterval(100); - ui.setupUi(this); - setAttribute(Qt::WA_NativeWindow, true); - ui.cameraName->addItems(get_camera_names()); + + for (const auto& str : video::camera_names()) + ui.cameraName->addItem(str); + tie_setting(s.camera_name, ui.cameraName); tie_setting(s.resolution, ui.resolution); - tie_setting(s.force_fps, ui.cameraFPS); tie_setting(s.fov, ui.cameraFOV); tie_setting(s.headpos_x, ui.cx); tie_setting(s.headpos_y, ui.cy); tie_setting(s.headpos_z, ui.cz); - - ui.model_rotation->addItem("0", int(settings::rot_zero)); - ui.model_rotation->addItem("+22.5", int(settings::rot_plus)); - ui.model_rotation->addItem("-22.5", int(settings::rot_neg)); - tie_setting(s.model_rotation, ui.model_rotation); + tie_setting(s.use_mjpeg, ui.use_mjpeg); connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); @@ -480,7 +538,7 @@ aruco_dialog::aruco_dialog() : connect(&calib_timer, SIGNAL(timeout()), this, SLOT(update_tracker_calibration())); 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&))); + connect(&s.camera_name, value_::value_changed<QString>(), this, &aruco_dialog::update_camera_settings_state); update_camera_settings_state(s.camera_name); } @@ -499,11 +557,11 @@ void aruco_dialog::toggleCalibrate() { cleanupCalib(); - cv::Vec3d pos; - std::tie(pos, std::ignore) = calibrator.get_estimate(); - s.headpos_x = -pos(0); - s.headpos_y = -pos(1); - s.headpos_z = -pos(2); + auto [ pos, nvals ] = calibrator.get_estimate(); + (void) nvals; + s.headpos_x = (double)-pos(0); + s.headpos_y = (double)-pos(1); + s.headpos_z = (double)-pos(2); } } @@ -540,15 +598,18 @@ 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)); + (void)tracker->camera->show_dialog(); } else - video_property_page::show(camera_name_to_index(s.camera_name)); + (void)video::show_dialog(s.camera_name); } void aruco_dialog::update_camera_settings_state(const QString& name) { - ui.camera_settings->setEnabled(video_property_page::should_show_dialog(name)); + (void)name; + ui.camera_settings->setEnabled(true); } +settings::settings() : opts("aruco-tracker") {} + OPENTRACK_DECLARE_TRACKER(aruco_tracker, aruco_dialog, aruco_metadata) |