diff options
Diffstat (limited to 'tracker-aruco/ftnoir_tracker_aruco.cpp')
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.cpp | 231 |
1 files changed, 140 insertions, 91 deletions
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index faa8bb44..5130a889 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -5,12 +5,10 @@ * 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) @@ -28,6 +26,7 @@ #include <QDebug> #include <vector> +#include <tuple> #include <cstdio> #include <cmath> #include <algorithm> @@ -40,9 +39,10 @@ static const int adaptive_sizes[] = 30, 80, #else + 5, 7, 9, - 13, + 11, #endif }; @@ -56,12 +56,15 @@ static const resolution_tuple resolution_choices[] = { { 640, 480 }, { 320, 240 }, + { 1280, 720 }, + { 1920, 1080 }, { 0, 0 } }; aruco_tracker::aruco_tracker() { - cv::setBreakOnError(true); + opencv_init(); + // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco. set_detector_params(); } @@ -72,7 +75,6 @@ aruco_tracker::~aruco_tracker() wait(); // fast start/stop causes breakage portable::sleep(1000); - camera.release(); } module_status aruco_tracker::start_tracker(QFrame* videoframe) @@ -81,8 +83,8 @@ module_status aruco_tracker::start_tracker(QFrame* videoframe) videoWidget = std::make_unique<cv_video_widget>(videoframe); layout = std::make_unique<QHBoxLayout>(); layout->setContentsMargins(0, 0, 0, 0); - layout->addWidget(videoWidget.get()); - videoframe->setLayout(layout.get()); + layout->addWidget(&*videoWidget); + videoframe->setLayout(&*layout); videoWidget->show(); start(); @@ -101,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]; @@ -130,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; - if (!camera.isOpened()) + args.use_mjpeg = s.use_mjpeg; + + if (!camera->start(args)) { qDebug() << "aruco tracker: can't open camera"; return false; @@ -182,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() @@ -217,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); } @@ -227,19 +237,6 @@ 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) - { - const double 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; @@ -249,12 +246,10 @@ void aruco_tracker::set_points() 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); @@ -268,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() @@ -341,9 +336,9 @@ void aruco_tracker::set_detector_params() 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); + detector._thresMethod = aruco::MarkerDetector::CANNY; + int value = adaptive_sizes[adaptive_size_pos]; + detector.setThresholdParams(value, value * 3); #endif } @@ -372,8 +367,6 @@ void aruco_tracker::cycle_detection_params() void aruco_tracker::run() { - cv::setNumThreads(1); - if (!open_camera()) return; @@ -385,11 +378,31 @@ void aruco_tracker::run() { 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_BGR2GRAY); + 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 { @@ -464,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())); @@ -492,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, base_value::value_changed<QString>(), this, &aruco_dialog::update_camera_settings_state); + connect(&s.camera_name, value_::value_changed<QString>(), this, &aruco_dialog::update_camera_settings_state); update_camera_settings_state(s.camera_name); } @@ -511,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); } } @@ -552,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) { + (void)name; ui.camera_settings->setEnabled(true); } +settings::settings() : opts("aruco-tracker") {} + OPENTRACK_DECLARE_TRACKER(aruco_tracker, aruco_dialog, aruco_metadata) |