From f20687d2d5f9b0542fedf0f45291024846d30d28 Mon Sep 17 00:00:00 2001 From: Stéphane Lenclud Date: Mon, 1 Apr 2019 21:10:53 +0200 Subject: Easy Tracker: Renaming some files --- tracker-easy/camera.h | 2 +- tracker-easy/frame.hpp | 2 +- tracker-easy/ftnoir_tracker_pt.cpp | 374 ------------------------------ tracker-easy/ftnoir_tracker_pt.h | 92 -------- tracker-easy/ftnoir_tracker_pt_dialog.cpp | 275 ---------------------- tracker-easy/ftnoir_tracker_pt_dialog.h | 55 ----- tracker-easy/module.cpp | 6 +- tracker-easy/point_extractor.h | 2 +- tracker-easy/pt-api.cpp | 54 ----- tracker-easy/pt-api.hpp | 128 ---------- tracker-easy/tracker-easy-api.cpp | 54 +++++ tracker-easy/tracker-easy-api.h | 128 ++++++++++ tracker-easy/tracker-easy-dialog.cpp | 275 ++++++++++++++++++++++ tracker-easy/tracker-easy-dialog.h | 55 +++++ tracker-easy/tracker-easy.cpp | 374 ++++++++++++++++++++++++++++++ tracker-easy/tracker-easy.h | 92 ++++++++ 16 files changed, 984 insertions(+), 984 deletions(-) delete mode 100644 tracker-easy/ftnoir_tracker_pt.cpp delete mode 100644 tracker-easy/ftnoir_tracker_pt.h delete mode 100644 tracker-easy/ftnoir_tracker_pt_dialog.cpp delete mode 100644 tracker-easy/ftnoir_tracker_pt_dialog.h delete mode 100644 tracker-easy/pt-api.cpp delete mode 100644 tracker-easy/pt-api.hpp create mode 100644 tracker-easy/tracker-easy-api.cpp create mode 100644 tracker-easy/tracker-easy-api.h create mode 100644 tracker-easy/tracker-easy-dialog.cpp create mode 100644 tracker-easy/tracker-easy-dialog.h create mode 100644 tracker-easy/tracker-easy.cpp create mode 100644 tracker-easy/tracker-easy.h diff --git a/tracker-easy/camera.h b/tracker-easy/camera.h index 65b0e552..5fec9a19 100644 --- a/tracker-easy/camera.h +++ b/tracker-easy/camera.h @@ -7,7 +7,7 @@ #pragma once -#include "pt-api.hpp" +#include "tracker-easy-api.h" #include "compat/timer.hpp" #include "video/camera.hpp" diff --git a/tracker-easy/frame.hpp b/tracker-easy/frame.hpp index 89334599..678665fd 100644 --- a/tracker-easy/frame.hpp +++ b/tracker-easy/frame.hpp @@ -1,6 +1,6 @@ #pragma once -#include "pt-api.hpp" +#include "tracker-easy-api.h" #include #include diff --git a/tracker-easy/ftnoir_tracker_pt.cpp b/tracker-easy/ftnoir_tracker_pt.cpp deleted file mode 100644 index 947970c3..00000000 --- a/tracker-easy/ftnoir_tracker_pt.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * Copyright (c) 2019 Stephane Lenclud - * - * 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 "ftnoir_tracker_pt.h" -#include "video/video-widget.hpp" -#include "compat/math-imports.hpp" -#include "compat/check-visible.hpp" - -#include "pt-api.hpp" - -#include -#include -#include -#include - -#include - -#include - -using namespace options; - -namespace pt_impl { - -EasyTracker::EasyTracker(pointer const& traits) : - traits { traits }, - s { traits->get_module_name() }, - point_extractor { traits->make_point_extractor() }, - camera { traits->make_camera() }, - frame { traits->make_frame() }, - preview_frame { traits->make_preview(preview_width, preview_height) } -{ - cv::setBreakOnError(true); - cv::setNumThreads(1); - - connect(s.b.get(), &bundle_::saving, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); - connect(s.b.get(), &bundle_::reloading, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); - - connect(&s.fov, value_::value_changed(), this, &EasyTracker::set_fov, Qt::DirectConnection); - set_fov(s.fov); -} - -EasyTracker::~EasyTracker() -{ - requestInterruption(); - wait(); - - QMutexLocker l(&camera_mtx); - camera->stop(); -} - - -// Compute Euler angles from ratation matrix -cv::Vec3f EulerAngles(cv::Mat &R) -{ - - float sy = sqrt(R.at(0, 0) * R.at(0, 0) + R.at(1, 0) * R.at(1, 0)); - - bool singular = sy < 1e-6; // If - - float x, y, z; - if (!singular) - { - x = atan2(R.at(2, 1), R.at(2, 2)); - y = atan2(-R.at(2, 0), sy); - z = atan2(R.at(1, 0), R.at(0, 0)); - } - else - { - x = atan2(-R.at(1, 2), R.at(1, 1)); - y = atan2(-R.at(2, 0), sy); - z = 0; - } - - // Convert to degrees - return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI); -} - - -void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles) -{ - - cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ; - double* _r = rotCamerMatrix.ptr(); - double projMatrix[12] = { _r[0],_r[1],_r[2],0, - _r[3],_r[4],_r[5],0, - _r[6],_r[7],_r[8],0 }; - - cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix), - cameraMatrix, - rotMatrix, - transVect, - rotMatrixX, - rotMatrixY, - rotMatrixZ, - eulerAngles); -} - - -void EasyTracker::run() -{ - maybe_reopen_camera(); - - while(!isInterruptionRequested()) - { - pt_camera_info info; - bool new_frame = false; - - { - QMutexLocker l(&camera_mtx); - - if (camera) - std::tie(new_frame, info) = camera->get_frame(*frame); - } - - if (new_frame) - { - const bool preview_visible = check_is_visible(); - - if (preview_visible) - *preview_frame = *frame; - - iImagePoints.clear(); - point_extractor->extract_points(*frame, *preview_frame, points, iImagePoints); - point_count.store(points.size(), std::memory_order_relaxed); - - const bool success = points.size() >= KPointCount; - - int topPointIndex = -1; - - { - QMutexLocker l(¢er_lock); - - if (success) - { - ever_success.store(true, std::memory_order_relaxed); - - // Solve P3P problem with OpenCV - - // Construct the points defining the object we want to detect based on settings. - // We are converting them from millimeters to centimeters. - // TODO: Need to support clip too. That's cap only for now. - // s.active_model_panel != PointModel::Clip - - std::vector objectPoints; - objectPoints.push_back(cv::Point3f(s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right - objectPoints.push_back(cv::Point3f(-s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left - objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top - - //Bitmap origin is top left - std::vector trackedPoints; - // Stuff bitmap point in there making sure they match the order of the object point - // Find top most point, that's the one with min Y as we assume our guy's head is not up side down - - int minY = std::numeric_limits::max(); - for (int i = 0; i < 3; i++) - { - if (iImagePoints[i][1] maxX) - { - maxX = iImagePoints[i][0]; - rightPointIndex = i; - } - } - - // Find left most point - int leftPointIndex = -1; - for (int i = 0; i < 3; i++) - { - // Excluding top most point - if (i != topPointIndex && i != rightPointIndex) - { - leftPointIndex = i; - break; - } - } - - // - trackedPoints.push_back(cv::Point2f(iImagePoints[rightPointIndex][0], iImagePoints[rightPointIndex][1])); - trackedPoints.push_back(cv::Point2f(iImagePoints[leftPointIndex][0], iImagePoints[leftPointIndex][1])); - trackedPoints.push_back(cv::Point2f(iImagePoints[topPointIndex][0], iImagePoints[topPointIndex][1])); - - std::cout << "Object: " << objectPoints << "\n"; - std::cout << "Points: " << trackedPoints << "\n"; - - - // Create our camera matrix - // TODO: Just do that once, use data member instead - // Double or Float? - cv::Mat cameraMatrix; - cameraMatrix.create(3, 3, CV_64FC1); - cameraMatrix.setTo(cv::Scalar(0)); - cameraMatrix.at(0, 0) = camera->info.focalLengthX; - cameraMatrix.at(1, 1) = camera->info.focalLengthY; - cameraMatrix.at(0, 2) = camera->info.principalPointX; - cameraMatrix.at(1, 2) = camera->info.principalPointY; - cameraMatrix.at(2, 2) = 1; - - // Create distortion cooefficients - cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1); - // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6 - distCoeffs.at(0, 0) = 0; // Radial first order - distCoeffs.at(1, 0) = camera->info.radialDistortionSecondOrder; // Radial second order - distCoeffs.at(2, 0) = 0; // Tangential first order - distCoeffs.at(3, 0) = 0; // Tangential second order - distCoeffs.at(4, 0) = 0; // Radial third order - distCoeffs.at(5, 0) = camera->info.radialDistortionFourthOrder; // Radial fourth order - distCoeffs.at(6, 0) = 0; // Radial fith order - distCoeffs.at(7, 0) = camera->info.radialDistortionSixthOrder; // Radial sixth order - - // Define our solution arrays - // They will receive up to 4 solutions for our P3P problem - - - // TODO: try SOLVEPNP_AP3P too - iAngles.clear(); - iBestSolutionIndex = -1; - int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P); - - if (solutionCount > 0) - { - std::cout << "Solution count: " << solutionCount << "\n"; - int minPitch = std::numeric_limits::max(); - // Find the solution we want - for (int i = 0; i < solutionCount; i++) - { - std::cout << "Translation:\n"; - std::cout << iTranslations.at(i); - std::cout << "\n"; - std::cout << "Rotation:\n"; - //std::cout << rvecs.at(i); - cv::Mat rotationCameraMatrix; - cv::Rodrigues(iRotations[i], rotationCameraMatrix); - cv::Vec3d angles; - getEulerAngles(rotationCameraMatrix,angles); - iAngles.push_back(angles); - - // Check if pitch is closest to zero - int absolutePitch = std::abs(angles[0]); - if (minPitch > absolutePitch) - { - minPitch = absolutePitch; - iBestSolutionIndex = i; - } - - //cv::Vec3f angles=EulerAngles(quaternion); - std::cout << angles; - std::cout << "\n"; - } - - std::cout << "\n"; - - } - - } - - // Send solution data back to main thread - QMutexLocker l2(&data_lock); - if (iBestSolutionIndex != -1) - { - iBestAngles = iAngles[iBestSolutionIndex]; - iBestTranslation = iTranslations[iBestSolutionIndex]; - } - - } - - if (preview_visible) - { - if (topPointIndex != -1) - { - // Render a cross to indicate which point is the head - preview_frame->draw_head_center(points[topPointIndex][0], points[topPointIndex][1]); - } - - widget->update_image(preview_frame->get_bitmap()); - - auto [ w, h ] = widget->preview_size(); - if (w != preview_width || h != preview_height) - { - preview_width = w; preview_height = h; - preview_frame = traits->make_preview(w, h); - } - } - } - } -} - -bool EasyTracker::maybe_reopen_camera() -{ - QMutexLocker l(&camera_mtx); - - return camera->start(s.camera_name, - s.cam_fps, s.cam_res_x, s.cam_res_y); -} - -void EasyTracker::set_fov(int value) -{ - QMutexLocker l(&camera_mtx); - camera->set_fov(value); -} - -module_status EasyTracker::start_tracker(QFrame* video_frame) -{ - //video_frame->setAttribute(Qt::WA_NativeWindow); - - widget = std::make_unique(video_frame); - layout = std::make_unique(video_frame); - layout->setContentsMargins(0, 0, 0, 0); - layout->addWidget(widget.get()); - video_frame->setLayout(layout.get()); - //video_widget->resize(video_frame->width(), video_frame->height()); - video_frame->show(); - - start(QThread::HighPriority); - - return {}; -} - -void EasyTracker::data(double *data) -{ - if (ever_success.load(std::memory_order_relaxed)) - { - // Get data back from tracker thread - QMutexLocker l(&data_lock); - data[Yaw] = iBestAngles[1]; - data[Pitch] = iBestAngles[0]; - data[Roll] = iBestAngles[2]; - data[TX] = iBestTranslation[0]; - data[TY] = iBestTranslation[1]; - data[TZ] = iBestTranslation[2]; - } -} - -bool EasyTracker::center() -{ - QMutexLocker l(¢er_lock); - //TODO: Do we need to do anything there? - return false; -} - -int EasyTracker::get_n_points() -{ - return (int)point_count.load(std::memory_order_relaxed); -} - -bool EasyTracker::get_cam_info(pt_camera_info& info) -{ - QMutexLocker l(&camera_mtx); - bool ret; - - std::tie(ret, info) = camera->get_info(); - return ret; -} - - -} // ns pt_impl diff --git a/tracker-easy/ftnoir_tracker_pt.h b/tracker-easy/ftnoir_tracker_pt.h deleted file mode 100644 index 0aba736c..00000000 --- a/tracker-easy/ftnoir_tracker_pt.h +++ /dev/null @@ -1,92 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * - * 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. - */ - -#pragma once - -#include "api/plugin-api.hpp" -#include "pt-api.hpp" -#include "cv/numeric.hpp" -#include "video/video-widget.hpp" - -#include -#include -#include - -#include - -#include -#include -#include - -namespace pt_impl { - -class EasyTrackerDialog; - -using namespace numeric_types; - -struct EasyTracker : QThread, ITracker -{ - friend class EasyTrackerDialog; - - template using pointer = pt_pointer; - - explicit EasyTracker(pointer const& pt_runtime_traits); - ~EasyTracker() override; - module_status start_tracker(QFrame* parent_window) override; - void data(double* data) override; - bool center() override; - - int get_n_points(); - [[nodiscard]] bool get_cam_info(pt_camera_info& info); - -private: - void run() override; - - bool maybe_reopen_camera(); - void set_fov(int value); - - pointer traits; - - QMutex camera_mtx; - - - pt_settings s; - - std::unique_ptr layout; - std::vector points; - std::vector iImagePoints; - - int preview_width = 320, preview_height = 240; - - pointer point_extractor; - pointer camera; - pointer widget; - pointer frame; - pointer preview_frame; - - std::atomic point_count { 0 }; - std::atomic ever_success = false; - mutable QMutex center_lock, data_lock; - - // Translation solutions - std::vector iTranslations; - // Rotation solutions - std::vector iRotations; - // Angle solutions, pitch, yaw, roll, in this order - std::vector iAngles; - // The index of our best solution in the above arrays - int iBestSolutionIndex = -1; - // Best translation - cv::Vec3d iBestTranslation; - // Best angles - cv::Vec3d iBestAngles; -}; - -} // ns pt_impl - -using Tracker_PT = pt_impl::EasyTracker; diff --git a/tracker-easy/ftnoir_tracker_pt_dialog.cpp b/tracker-easy/ftnoir_tracker_pt_dialog.cpp deleted file mode 100644 index a3fa9470..00000000 --- a/tracker-easy/ftnoir_tracker_pt_dialog.cpp +++ /dev/null @@ -1,275 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2015 Stanislaw Halik - * - * 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 "ftnoir_tracker_pt_dialog.h" -#include "compat/math.hpp" -#include "video/camera.hpp" - -#include - -#include -#include -#include - -using namespace options; - -static void init_resources() { Q_INIT_RESOURCE(tracker_easy); } - -namespace pt_impl { - -EasyTrackerDialog::EasyTrackerDialog(const QString& module_name) : - s(module_name), - tracker(nullptr), - timer(this), - trans_calib(1, 2) -{ - init_resources(); - - ui.setupUi(this); - - for (const QString& str : video::camera_names()) - ui.camdevice_combo->addItem(str); - - tie_setting(s.camera_name, ui.camdevice_combo); - tie_setting(s.cam_res_x, ui.res_x_spin); - tie_setting(s.cam_res_y, ui.res_y_spin); - tie_setting(s.cam_fps, ui.fps_spin); - - tie_setting(s.threshold_slider, ui.threshold_slider); - - tie_setting(s.min_point_size, ui.mindiam_spin); - tie_setting(s.max_point_size, ui.maxdiam_spin); - - tie_setting(s.clip_by, ui.clip_bheight_spin); - tie_setting(s.clip_bz, ui.clip_blength_spin); - tie_setting(s.clip_ty, ui.clip_theight_spin); - tie_setting(s.clip_tz, ui.clip_tlength_spin); - - tie_setting(s.cap_x, ui.cap_width_spin); - tie_setting(s.cap_y, ui.cap_height_spin); - tie_setting(s.cap_z, ui.cap_length_spin); - - tie_setting(s.m01_x, ui.m1x_spin); - tie_setting(s.m01_y, ui.m1y_spin); - tie_setting(s.m01_z, ui.m1z_spin); - - tie_setting(s.m02_x, ui.m2x_spin); - tie_setting(s.m02_y, ui.m2y_spin); - tie_setting(s.m02_z, ui.m2z_spin); - - tie_setting(s.t_MH_x, ui.tx_spin); - tie_setting(s.t_MH_y, ui.ty_spin); - tie_setting(s.t_MH_z, ui.tz_spin); - - tie_setting(s.fov, ui.fov); - - tie_setting(s.active_model_panel, ui.model_tabs); - - tie_setting(s.dynamic_pose, ui.dynamic_pose); - tie_setting(s.init_phase_timeout, ui.init_phase_timeout); - - tie_setting(s.auto_threshold, ui.auto_threshold); - - connect(ui.tcalib_button,SIGNAL(toggled(bool)), this, SLOT(startstop_trans_calib(bool))); - - connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); - connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - - connect(ui.camdevice_combo, &QComboBox::currentTextChanged, this, &EasyTrackerDialog::set_camera_settings_available); - set_camera_settings_available(ui.camdevice_combo->currentText()); - connect(ui.camera_settings, &QPushButton::clicked, this, &EasyTrackerDialog::show_camera_settings); - - connect(&timer, &QTimer::timeout, this, &EasyTrackerDialog::poll_tracker_info_impl); - timer.setInterval(250); - - connect(&calib_timer, &QTimer::timeout, this, &EasyTrackerDialog::trans_calib_step); - calib_timer.setInterval(35); - - poll_tracker_info_impl(); - - connect(this, &EasyTrackerDialog::poll_tracker_info, this, &EasyTrackerDialog::poll_tracker_info_impl, Qt::DirectConnection); - - constexpr pt_color_type color_types[] = { - pt_color_average, - pt_color_natural, - pt_color_red_only, - pt_color_green_only, - pt_color_blue_only, - }; - - for (unsigned k = 0; k < std::size(color_types); k++) - ui.blob_color->setItemData(k, int(color_types[k])); - - tie_setting(s.blob_color, ui.blob_color); - - tie_setting(s.threshold_slider, ui.threshold_value_display, [this](const slider_value& val) { - return threshold_display_text(int(val)); - }); - - // refresh threshold display on auto-threshold checkbox state change - tie_setting(s.auto_threshold, - this, - [this](bool) { s.threshold_slider.notify(); }); -} - -QString EasyTrackerDialog::threshold_display_text(int threshold_value) -{ - if (!s.auto_threshold) - return tr("Brightness %1/255").arg(threshold_value); - else - { - pt_camera_info info; - int w = s.cam_res_x, h = s.cam_res_y; - - if (w * h <= 0) - { - w = 640; - h = 480; - } - - if (tracker && tracker->get_cam_info(info) && info.res_x * info.res_y != 0) - { - w = info.res_x; - h = info.res_y; - } - - double value = (double)pt_point_extractor::threshold_radius_value(w, h, threshold_value); - - return tr("LED radius %1 pixels").arg(value, 0, 'f', 2); - } -} - -void EasyTrackerDialog::startstop_trans_calib(bool start) -{ - QMutexLocker l(&calibrator_mutex); - - if (start) - { - qDebug() << "pt: starting translation calibration"; - calib_timer.start(); - trans_calib.reset(); - s.t_MH_x = 0; - s.t_MH_y = 0; - s.t_MH_z = 0; - - ui.sample_count_display->setText(QString()); - } - else - { - calib_timer.stop(); - qDebug() << "pt: stopping translation calibration"; - { - auto [tmp, nsamples] = trans_calib.get_estimate(); - s.t_MH_x = int(tmp[0]); - s.t_MH_y = int(tmp[1]); - s.t_MH_z = int(tmp[2]); - - constexpr int min_yaw_samples = 15; - constexpr int min_pitch_samples = 15; - constexpr int min_samples = min_yaw_samples+min_pitch_samples; - - // Don't bother counting roll samples. Roll calibration is hard enough - // that it's a hidden unsupported feature anyway. - - QString sample_feedback; - if (nsamples[0] < min_yaw_samples) - sample_feedback = tr("%1 yaw samples. Yaw more to %2 samples for stable calibration.").arg(nsamples[0]).arg(min_yaw_samples); - else if (nsamples[1] < min_pitch_samples) - sample_feedback = tr("%1 pitch samples. Pitch more to %2 samples for stable calibration.").arg(nsamples[1]).arg(min_pitch_samples); - else - { - const int nsamples_total = nsamples[0] + nsamples[1]; - sample_feedback = tr("%1 samples. Over %2, good!").arg(nsamples_total).arg(min_samples); - } - - ui.sample_count_display->setText(sample_feedback); - } - } - ui.tx_spin->setEnabled(!start); - ui.ty_spin->setEnabled(!start); - ui.tz_spin->setEnabled(!start); - - if (start) - ui.tcalib_button->setText(tr("Stop calibration")); - else - ui.tcalib_button->setText(tr("Start calibration")); -} - -void EasyTrackerDialog::poll_tracker_info_impl() -{ - pt_camera_info info; - if (tracker && tracker->get_cam_info(info)) - { - ui.caminfo_label->setText(tr("%1x%2 @ %3 FPS").arg(info.res_x).arg(info.res_y).arg(iround(info.fps))); - - // display point info - const int n_points = tracker->get_n_points(); - ui.pointinfo_label->setText((n_points == 3 ? tr("%1 OK!") : tr("%1 BAD!")).arg(n_points)); - } - else - { - ui.caminfo_label->setText(tr("Tracker offline")); - ui.pointinfo_label->setText(QString()); - } -} - -void EasyTrackerDialog::set_camera_settings_available(const QString& /* camera_name */) -{ - ui.camera_settings->setEnabled(true); -} - -void EasyTrackerDialog::show_camera_settings() -{ - if (tracker) - { - QMutexLocker l(&tracker->camera_mtx); - tracker->camera->show_camera_settings(); - } - else - (void)video::show_dialog(s.camera_name); -} - -void EasyTrackerDialog::trans_calib_step() -{ - QMutexLocker l(&calibrator_mutex); - // TODO: Do we still need that function -} - -void EasyTrackerDialog::save() -{ - s.b->save(); -} - -void EasyTrackerDialog::doOK() -{ - save(); - close(); -} - -void EasyTrackerDialog::doCancel() -{ - close(); -} - -void EasyTrackerDialog::register_tracker(ITracker *t) -{ - tracker = static_cast(t); - ui.tcalib_button->setEnabled(true); - poll_tracker_info(); - timer.start(); -} - -void EasyTrackerDialog::unregister_tracker() -{ - tracker = nullptr; - ui.tcalib_button->setEnabled(false); - poll_tracker_info(); - timer.stop(); -} - -} // ns pt_impl diff --git a/tracker-easy/ftnoir_tracker_pt_dialog.h b/tracker-easy/ftnoir_tracker_pt_dialog.h deleted file mode 100644 index 4f6d956f..00000000 --- a/tracker-easy/ftnoir_tracker_pt_dialog.h +++ /dev/null @@ -1,55 +0,0 @@ -/* 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. - */ - -#pragma once - -#include "pt-api.hpp" - -#include "ftnoir_tracker_pt.h" -#include "tracker-pt/ui_FTNoIR_PT_Controls.h" -#include "cv/translation-calibrator.hpp" -#include "video/video-widget.hpp" - -#include -#include - -namespace pt_impl { - -class EasyTrackerDialog : public ITrackerDialog -{ - Q_OBJECT -public: - EasyTrackerDialog(const QString& module_name); - void register_tracker(ITracker *tracker) override; - void unregister_tracker() override; - void save(); -public slots: - void doOK(); - void doCancel(); - - void startstop_trans_calib(bool start); - void trans_calib_step(); - void poll_tracker_info_impl(); - void set_camera_settings_available(const QString& camera_name); - void show_camera_settings(); -signals: - void poll_tracker_info(); -protected: - QString threshold_display_text(int threshold_value); - - pt_settings s; - EasyTracker* tracker; - QTimer timer, calib_timer; - TranslationCalibrator trans_calib; - QMutex calibrator_mutex; - - Ui::UICPTClientControls ui; -}; - -} // ns pt_impl - -using TrackerDialog_PT = pt_impl::EasyTrackerDialog; diff --git a/tracker-easy/module.cpp b/tracker-easy/module.cpp index 9aa71385..10d25369 100644 --- a/tracker-easy/module.cpp +++ b/tracker-easy/module.cpp @@ -1,12 +1,12 @@ -#include "ftnoir_tracker_pt.h" +#include "tracker-easy.h" +#include "tracker-easy-dialog.h" +#include "tracker-easy-api.h" #include "module.hpp" #include "camera.h" #include "frame.hpp" #include "point_extractor.h" -#include "ftnoir_tracker_pt_dialog.h" -#include "pt-api.hpp" #include diff --git a/tracker-easy/point_extractor.h b/tracker-easy/point_extractor.h index 2af5c131..fd0b8144 100644 --- a/tracker-easy/point_extractor.h +++ b/tracker-easy/point_extractor.h @@ -8,7 +8,7 @@ #pragma once -#include "pt-api.hpp" +#include "tracker-easy-api.h" #include diff --git a/tracker-easy/pt-api.cpp b/tracker-easy/pt-api.cpp deleted file mode 100644 index f64d5c9a..00000000 --- a/tracker-easy/pt-api.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "pt-api.hpp" -#include "cv/numeric.hpp" - -using namespace numeric_types; - -pt_camera_info::pt_camera_info() = default; - -f pt_camera_info::get_focal_length(f fov, int res_x, int res_y) -{ - const f diag_len = std::sqrt(f(res_x*res_x + res_y*res_y)); - const f aspect_x = res_x / diag_len; - //const double aspect_y = res_y / diag_len; - const f diag_fov = fov * pi/180; - const f fov_x = 2*std::atan(std::tan(diag_fov*f{.5}) * aspect_x); - //const double fov_y = 2*atan(tan(diag_fov*.5) * aspect_y); - const f fx = f{.5} / std::tan(fov_x * f{.5}); - return fx; - //fy = .5 / tan(fov_y * .5); - //static bool once = false; if (!once) { once = true; qDebug() << "f" << ret << "fov" << (fov * 180/M_PI); } -} - -pt_camera::pt_camera() = default; -pt_camera::~pt_camera() = default; -pt_runtime_traits::pt_runtime_traits() = default; -pt_runtime_traits::~pt_runtime_traits() = default; -pt_point_extractor::pt_point_extractor() = default; -pt_point_extractor::~pt_point_extractor() = default; - -f pt_point_extractor::threshold_radius_value(int w, int h, int threshold) -{ - f cx = w / f{640}, cy = h / f{480}; - - const f min_radius = f{1.75} * cx; - const f max_radius = f{15} * cy; - - const f radius = std::fmax(f{0}, (max_radius-min_radius) * threshold / f(255) + min_radius); - - return radius; -} - -std::tuple pt_pixel_pos_mixin::to_pixel_pos(f x, f y, int w, int h) -{ - return std::make_tuple(w*(x+f{.5}), f{.5}*(h - 2*y*w)); -} - -std::tuple pt_pixel_pos_mixin::to_screen_pos(f px, f py, int w, int h) -{ - px *= w/(w-f{1}); py *= h/(h-f{1}); - return std::make_tuple((px - w/f{2})/w, -(py - h/f{2})/w); -} - -pt_frame::pt_frame() = default; - -pt_frame::~pt_frame() = default; diff --git a/tracker-easy/pt-api.hpp b/tracker-easy/pt-api.hpp deleted file mode 100644 index 81a52f7f..00000000 --- a/tracker-easy/pt-api.hpp +++ /dev/null @@ -1,128 +0,0 @@ -#pragma once - -#include "pt-settings.hpp" - -#include "cv/numeric.hpp" -#include "options/options.hpp" -#include "video/camera.hpp" - -#include -#include -#include - -#include - -#include -#include - -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wweak-vtables" -#endif - -const int KPointCount = 3; - -struct pt_camera_info final -{ - using f = numeric_types::f; - - pt_camera_info(); - static f get_focal_length(f fov, int res_x, int res_y); - - f fov = 0; - f fps = 0; - - int res_x = 0; - int res_y = 0; - QString name; -}; - -struct pt_pixel_pos_mixin -{ - using f = numeric_types::f; - - static std::tuple to_pixel_pos(f x, f y, int w, int h); - static std::tuple to_screen_pos(f px, f py, int w, int h); -}; - -struct pt_frame : pt_pixel_pos_mixin -{ - pt_frame(); - virtual ~pt_frame(); - - template - t* as() & - { - return static_cast(this); - } - - template - t const* as_const() const& - { - return static_cast(this); - } -}; - -struct pt_preview : pt_frame -{ - virtual pt_preview& operator=(const pt_frame&) = 0; - virtual QImage get_bitmap() = 0; - virtual void draw_head_center(f x, f y) = 0; -}; - -struct pt_camera -{ - using result = std::tuple; - using f = numeric_types::f; - - pt_camera(); - virtual ~pt_camera(); - - [[nodiscard]] virtual bool start(const QString& name, int fps, int res_x, int res_y) = 0; - virtual void stop() = 0; - - virtual result get_frame(pt_frame& frame) = 0; - virtual result get_info() const = 0; - virtual pt_camera_info get_desired() const = 0; - - virtual QString get_desired_name() const = 0; - virtual QString get_active_name() const = 0; - - virtual void set_fov(f value) = 0; - virtual void show_camera_settings() = 0; - - video::impl::camera::info info; -}; - -struct pt_point_extractor : pt_pixel_pos_mixin -{ - using vec2 = numeric_types::vec2; - using f = numeric_types::f; - - pt_point_extractor(); - virtual ~pt_point_extractor(); - virtual void extract_points(const pt_frame& image, pt_preview& preview_frame, std::vector& points, std::vector& imagePoints) = 0; - - static f threshold_radius_value(int w, int h, int threshold); -}; - -struct pt_runtime_traits -{ - template using pointer = std::shared_ptr; - - pt_runtime_traits(); - virtual ~pt_runtime_traits(); - - virtual pointer make_camera() const = 0; - virtual pointer make_point_extractor() const = 0; - virtual pointer make_frame() const = 0; - virtual pointer make_preview(int w, int h) const = 0; - virtual QString get_module_name() const = 0; -}; - -template -using pt_pointer = typename pt_runtime_traits::pointer; - -#ifdef __clang__ -# pragma clang diagnostic pop -#endif diff --git a/tracker-easy/tracker-easy-api.cpp b/tracker-easy/tracker-easy-api.cpp new file mode 100644 index 00000000..49e11759 --- /dev/null +++ b/tracker-easy/tracker-easy-api.cpp @@ -0,0 +1,54 @@ +#include "tracker-easy-api.h" +#include "cv/numeric.hpp" + +using namespace numeric_types; + +pt_camera_info::pt_camera_info() = default; + +f pt_camera_info::get_focal_length(f fov, int res_x, int res_y) +{ + const f diag_len = std::sqrt(f(res_x*res_x + res_y*res_y)); + const f aspect_x = res_x / diag_len; + //const double aspect_y = res_y / diag_len; + const f diag_fov = fov * pi/180; + const f fov_x = 2*std::atan(std::tan(diag_fov*f{.5}) * aspect_x); + //const double fov_y = 2*atan(tan(diag_fov*.5) * aspect_y); + const f fx = f{.5} / std::tan(fov_x * f{.5}); + return fx; + //fy = .5 / tan(fov_y * .5); + //static bool once = false; if (!once) { once = true; qDebug() << "f" << ret << "fov" << (fov * 180/M_PI); } +} + +pt_camera::pt_camera() = default; +pt_camera::~pt_camera() = default; +pt_runtime_traits::pt_runtime_traits() = default; +pt_runtime_traits::~pt_runtime_traits() = default; +pt_point_extractor::pt_point_extractor() = default; +pt_point_extractor::~pt_point_extractor() = default; + +f pt_point_extractor::threshold_radius_value(int w, int h, int threshold) +{ + f cx = w / f{640}, cy = h / f{480}; + + const f min_radius = f{1.75} * cx; + const f max_radius = f{15} * cy; + + const f radius = std::fmax(f{0}, (max_radius-min_radius) * threshold / f(255) + min_radius); + + return radius; +} + +std::tuple pt_pixel_pos_mixin::to_pixel_pos(f x, f y, int w, int h) +{ + return std::make_tuple(w*(x+f{.5}), f{.5}*(h - 2*y*w)); +} + +std::tuple pt_pixel_pos_mixin::to_screen_pos(f px, f py, int w, int h) +{ + px *= w/(w-f{1}); py *= h/(h-f{1}); + return std::make_tuple((px - w/f{2})/w, -(py - h/f{2})/w); +} + +pt_frame::pt_frame() = default; + +pt_frame::~pt_frame() = default; diff --git a/tracker-easy/tracker-easy-api.h b/tracker-easy/tracker-easy-api.h new file mode 100644 index 00000000..81a52f7f --- /dev/null +++ b/tracker-easy/tracker-easy-api.h @@ -0,0 +1,128 @@ +#pragma once + +#include "pt-settings.hpp" + +#include "cv/numeric.hpp" +#include "options/options.hpp" +#include "video/camera.hpp" + +#include +#include +#include + +#include + +#include +#include + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +const int KPointCount = 3; + +struct pt_camera_info final +{ + using f = numeric_types::f; + + pt_camera_info(); + static f get_focal_length(f fov, int res_x, int res_y); + + f fov = 0; + f fps = 0; + + int res_x = 0; + int res_y = 0; + QString name; +}; + +struct pt_pixel_pos_mixin +{ + using f = numeric_types::f; + + static std::tuple to_pixel_pos(f x, f y, int w, int h); + static std::tuple to_screen_pos(f px, f py, int w, int h); +}; + +struct pt_frame : pt_pixel_pos_mixin +{ + pt_frame(); + virtual ~pt_frame(); + + template + t* as() & + { + return static_cast(this); + } + + template + t const* as_const() const& + { + return static_cast(this); + } +}; + +struct pt_preview : pt_frame +{ + virtual pt_preview& operator=(const pt_frame&) = 0; + virtual QImage get_bitmap() = 0; + virtual void draw_head_center(f x, f y) = 0; +}; + +struct pt_camera +{ + using result = std::tuple; + using f = numeric_types::f; + + pt_camera(); + virtual ~pt_camera(); + + [[nodiscard]] virtual bool start(const QString& name, int fps, int res_x, int res_y) = 0; + virtual void stop() = 0; + + virtual result get_frame(pt_frame& frame) = 0; + virtual result get_info() const = 0; + virtual pt_camera_info get_desired() const = 0; + + virtual QString get_desired_name() const = 0; + virtual QString get_active_name() const = 0; + + virtual void set_fov(f value) = 0; + virtual void show_camera_settings() = 0; + + video::impl::camera::info info; +}; + +struct pt_point_extractor : pt_pixel_pos_mixin +{ + using vec2 = numeric_types::vec2; + using f = numeric_types::f; + + pt_point_extractor(); + virtual ~pt_point_extractor(); + virtual void extract_points(const pt_frame& image, pt_preview& preview_frame, std::vector& points, std::vector& imagePoints) = 0; + + static f threshold_radius_value(int w, int h, int threshold); +}; + +struct pt_runtime_traits +{ + template using pointer = std::shared_ptr; + + pt_runtime_traits(); + virtual ~pt_runtime_traits(); + + virtual pointer make_camera() const = 0; + virtual pointer make_point_extractor() const = 0; + virtual pointer make_frame() const = 0; + virtual pointer make_preview(int w, int h) const = 0; + virtual QString get_module_name() const = 0; +}; + +template +using pt_pointer = typename pt_runtime_traits::pointer; + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif diff --git a/tracker-easy/tracker-easy-dialog.cpp b/tracker-easy/tracker-easy-dialog.cpp new file mode 100644 index 00000000..2a5654e3 --- /dev/null +++ b/tracker-easy/tracker-easy-dialog.cpp @@ -0,0 +1,275 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik + * + * 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 "tracker-easy-dialog.h" +#include "compat/math.hpp" +#include "video/camera.hpp" + +#include + +#include +#include +#include + +using namespace options; + +static void init_resources() { Q_INIT_RESOURCE(tracker_easy); } + +namespace pt_impl { + +EasyTrackerDialog::EasyTrackerDialog(const QString& module_name) : + s(module_name), + tracker(nullptr), + timer(this), + trans_calib(1, 2) +{ + init_resources(); + + ui.setupUi(this); + + for (const QString& str : video::camera_names()) + ui.camdevice_combo->addItem(str); + + tie_setting(s.camera_name, ui.camdevice_combo); + tie_setting(s.cam_res_x, ui.res_x_spin); + tie_setting(s.cam_res_y, ui.res_y_spin); + tie_setting(s.cam_fps, ui.fps_spin); + + tie_setting(s.threshold_slider, ui.threshold_slider); + + tie_setting(s.min_point_size, ui.mindiam_spin); + tie_setting(s.max_point_size, ui.maxdiam_spin); + + tie_setting(s.clip_by, ui.clip_bheight_spin); + tie_setting(s.clip_bz, ui.clip_blength_spin); + tie_setting(s.clip_ty, ui.clip_theight_spin); + tie_setting(s.clip_tz, ui.clip_tlength_spin); + + tie_setting(s.cap_x, ui.cap_width_spin); + tie_setting(s.cap_y, ui.cap_height_spin); + tie_setting(s.cap_z, ui.cap_length_spin); + + tie_setting(s.m01_x, ui.m1x_spin); + tie_setting(s.m01_y, ui.m1y_spin); + tie_setting(s.m01_z, ui.m1z_spin); + + tie_setting(s.m02_x, ui.m2x_spin); + tie_setting(s.m02_y, ui.m2y_spin); + tie_setting(s.m02_z, ui.m2z_spin); + + tie_setting(s.t_MH_x, ui.tx_spin); + tie_setting(s.t_MH_y, ui.ty_spin); + tie_setting(s.t_MH_z, ui.tz_spin); + + tie_setting(s.fov, ui.fov); + + tie_setting(s.active_model_panel, ui.model_tabs); + + tie_setting(s.dynamic_pose, ui.dynamic_pose); + tie_setting(s.init_phase_timeout, ui.init_phase_timeout); + + tie_setting(s.auto_threshold, ui.auto_threshold); + + connect(ui.tcalib_button,SIGNAL(toggled(bool)), this, SLOT(startstop_trans_calib(bool))); + + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + + connect(ui.camdevice_combo, &QComboBox::currentTextChanged, this, &EasyTrackerDialog::set_camera_settings_available); + set_camera_settings_available(ui.camdevice_combo->currentText()); + connect(ui.camera_settings, &QPushButton::clicked, this, &EasyTrackerDialog::show_camera_settings); + + connect(&timer, &QTimer::timeout, this, &EasyTrackerDialog::poll_tracker_info_impl); + timer.setInterval(250); + + connect(&calib_timer, &QTimer::timeout, this, &EasyTrackerDialog::trans_calib_step); + calib_timer.setInterval(35); + + poll_tracker_info_impl(); + + connect(this, &EasyTrackerDialog::poll_tracker_info, this, &EasyTrackerDialog::poll_tracker_info_impl, Qt::DirectConnection); + + constexpr pt_color_type color_types[] = { + pt_color_average, + pt_color_natural, + pt_color_red_only, + pt_color_green_only, + pt_color_blue_only, + }; + + for (unsigned k = 0; k < std::size(color_types); k++) + ui.blob_color->setItemData(k, int(color_types[k])); + + tie_setting(s.blob_color, ui.blob_color); + + tie_setting(s.threshold_slider, ui.threshold_value_display, [this](const slider_value& val) { + return threshold_display_text(int(val)); + }); + + // refresh threshold display on auto-threshold checkbox state change + tie_setting(s.auto_threshold, + this, + [this](bool) { s.threshold_slider.notify(); }); +} + +QString EasyTrackerDialog::threshold_display_text(int threshold_value) +{ + if (!s.auto_threshold) + return tr("Brightness %1/255").arg(threshold_value); + else + { + pt_camera_info info; + int w = s.cam_res_x, h = s.cam_res_y; + + if (w * h <= 0) + { + w = 640; + h = 480; + } + + if (tracker && tracker->get_cam_info(info) && info.res_x * info.res_y != 0) + { + w = info.res_x; + h = info.res_y; + } + + double value = (double)pt_point_extractor::threshold_radius_value(w, h, threshold_value); + + return tr("LED radius %1 pixels").arg(value, 0, 'f', 2); + } +} + +void EasyTrackerDialog::startstop_trans_calib(bool start) +{ + QMutexLocker l(&calibrator_mutex); + + if (start) + { + qDebug() << "pt: starting translation calibration"; + calib_timer.start(); + trans_calib.reset(); + s.t_MH_x = 0; + s.t_MH_y = 0; + s.t_MH_z = 0; + + ui.sample_count_display->setText(QString()); + } + else + { + calib_timer.stop(); + qDebug() << "pt: stopping translation calibration"; + { + auto [tmp, nsamples] = trans_calib.get_estimate(); + s.t_MH_x = int(tmp[0]); + s.t_MH_y = int(tmp[1]); + s.t_MH_z = int(tmp[2]); + + constexpr int min_yaw_samples = 15; + constexpr int min_pitch_samples = 15; + constexpr int min_samples = min_yaw_samples+min_pitch_samples; + + // Don't bother counting roll samples. Roll calibration is hard enough + // that it's a hidden unsupported feature anyway. + + QString sample_feedback; + if (nsamples[0] < min_yaw_samples) + sample_feedback = tr("%1 yaw samples. Yaw more to %2 samples for stable calibration.").arg(nsamples[0]).arg(min_yaw_samples); + else if (nsamples[1] < min_pitch_samples) + sample_feedback = tr("%1 pitch samples. Pitch more to %2 samples for stable calibration.").arg(nsamples[1]).arg(min_pitch_samples); + else + { + const int nsamples_total = nsamples[0] + nsamples[1]; + sample_feedback = tr("%1 samples. Over %2, good!").arg(nsamples_total).arg(min_samples); + } + + ui.sample_count_display->setText(sample_feedback); + } + } + ui.tx_spin->setEnabled(!start); + ui.ty_spin->setEnabled(!start); + ui.tz_spin->setEnabled(!start); + + if (start) + ui.tcalib_button->setText(tr("Stop calibration")); + else + ui.tcalib_button->setText(tr("Start calibration")); +} + +void EasyTrackerDialog::poll_tracker_info_impl() +{ + pt_camera_info info; + if (tracker && tracker->get_cam_info(info)) + { + ui.caminfo_label->setText(tr("%1x%2 @ %3 FPS").arg(info.res_x).arg(info.res_y).arg(iround(info.fps))); + + // display point info + const int n_points = tracker->get_n_points(); + ui.pointinfo_label->setText((n_points == 3 ? tr("%1 OK!") : tr("%1 BAD!")).arg(n_points)); + } + else + { + ui.caminfo_label->setText(tr("Tracker offline")); + ui.pointinfo_label->setText(QString()); + } +} + +void EasyTrackerDialog::set_camera_settings_available(const QString& /* camera_name */) +{ + ui.camera_settings->setEnabled(true); +} + +void EasyTrackerDialog::show_camera_settings() +{ + if (tracker) + { + QMutexLocker l(&tracker->camera_mtx); + tracker->camera->show_camera_settings(); + } + else + (void)video::show_dialog(s.camera_name); +} + +void EasyTrackerDialog::trans_calib_step() +{ + QMutexLocker l(&calibrator_mutex); + // TODO: Do we still need that function +} + +void EasyTrackerDialog::save() +{ + s.b->save(); +} + +void EasyTrackerDialog::doOK() +{ + save(); + close(); +} + +void EasyTrackerDialog::doCancel() +{ + close(); +} + +void EasyTrackerDialog::register_tracker(ITracker *t) +{ + tracker = static_cast(t); + ui.tcalib_button->setEnabled(true); + poll_tracker_info(); + timer.start(); +} + +void EasyTrackerDialog::unregister_tracker() +{ + tracker = nullptr; + ui.tcalib_button->setEnabled(false); + poll_tracker_info(); + timer.stop(); +} + +} // ns pt_impl diff --git a/tracker-easy/tracker-easy-dialog.h b/tracker-easy/tracker-easy-dialog.h new file mode 100644 index 00000000..24f1ff84 --- /dev/null +++ b/tracker-easy/tracker-easy-dialog.h @@ -0,0 +1,55 @@ +/* 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. + */ + +#pragma once + +#include "tracker-easy-api.h" + +#include "tracker-easy.h" +#include "ui_FTNoIR_PT_Controls.h" +#include "cv/translation-calibrator.hpp" +#include "video/video-widget.hpp" + +#include +#include + +namespace pt_impl { + +class EasyTrackerDialog : public ITrackerDialog +{ + Q_OBJECT +public: + EasyTrackerDialog(const QString& module_name); + void register_tracker(ITracker *tracker) override; + void unregister_tracker() override; + void save(); +public slots: + void doOK(); + void doCancel(); + + void startstop_trans_calib(bool start); + void trans_calib_step(); + void poll_tracker_info_impl(); + void set_camera_settings_available(const QString& camera_name); + void show_camera_settings(); +signals: + void poll_tracker_info(); +protected: + QString threshold_display_text(int threshold_value); + + pt_settings s; + EasyTracker* tracker; + QTimer timer, calib_timer; + TranslationCalibrator trans_calib; + QMutex calibrator_mutex; + + Ui::UICPTClientControls ui; +}; + +} // ns pt_impl + +using TrackerDialog_PT = pt_impl::EasyTrackerDialog; diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp new file mode 100644 index 00000000..c30bb40f --- /dev/null +++ b/tracker-easy/tracker-easy.cpp @@ -0,0 +1,374 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * Copyright (c) 2019 Stephane Lenclud + * + * 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 "tracker-easy.h" +#include "video/video-widget.hpp" +#include "compat/math-imports.hpp" +#include "compat/check-visible.hpp" + +#include "tracker-easy-api.h" + +#include +#include +#include +#include + +#include + +#include + +using namespace options; + +namespace pt_impl { + +EasyTracker::EasyTracker(pointer const& traits) : + traits { traits }, + s { traits->get_module_name() }, + point_extractor { traits->make_point_extractor() }, + camera { traits->make_camera() }, + frame { traits->make_frame() }, + preview_frame { traits->make_preview(preview_width, preview_height) } +{ + cv::setBreakOnError(true); + cv::setNumThreads(1); + + connect(s.b.get(), &bundle_::saving, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); + connect(s.b.get(), &bundle_::reloading, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); + + connect(&s.fov, value_::value_changed(), this, &EasyTracker::set_fov, Qt::DirectConnection); + set_fov(s.fov); +} + +EasyTracker::~EasyTracker() +{ + requestInterruption(); + wait(); + + QMutexLocker l(&camera_mtx); + camera->stop(); +} + + +// Compute Euler angles from ratation matrix +cv::Vec3f EulerAngles(cv::Mat &R) +{ + + float sy = sqrt(R.at(0, 0) * R.at(0, 0) + R.at(1, 0) * R.at(1, 0)); + + bool singular = sy < 1e-6; // If + + float x, y, z; + if (!singular) + { + x = atan2(R.at(2, 1), R.at(2, 2)); + y = atan2(-R.at(2, 0), sy); + z = atan2(R.at(1, 0), R.at(0, 0)); + } + else + { + x = atan2(-R.at(1, 2), R.at(1, 1)); + y = atan2(-R.at(2, 0), sy); + z = 0; + } + + // Convert to degrees + return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI); +} + + +void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles) +{ + + cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ; + double* _r = rotCamerMatrix.ptr(); + double projMatrix[12] = { _r[0],_r[1],_r[2],0, + _r[3],_r[4],_r[5],0, + _r[6],_r[7],_r[8],0 }; + + cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix), + cameraMatrix, + rotMatrix, + transVect, + rotMatrixX, + rotMatrixY, + rotMatrixZ, + eulerAngles); +} + + +void EasyTracker::run() +{ + maybe_reopen_camera(); + + while(!isInterruptionRequested()) + { + pt_camera_info info; + bool new_frame = false; + + { + QMutexLocker l(&camera_mtx); + + if (camera) + std::tie(new_frame, info) = camera->get_frame(*frame); + } + + if (new_frame) + { + const bool preview_visible = check_is_visible(); + + if (preview_visible) + *preview_frame = *frame; + + iImagePoints.clear(); + point_extractor->extract_points(*frame, *preview_frame, points, iImagePoints); + point_count.store(points.size(), std::memory_order_relaxed); + + const bool success = points.size() >= KPointCount; + + int topPointIndex = -1; + + { + QMutexLocker l(¢er_lock); + + if (success) + { + ever_success.store(true, std::memory_order_relaxed); + + // Solve P3P problem with OpenCV + + // Construct the points defining the object we want to detect based on settings. + // We are converting them from millimeters to centimeters. + // TODO: Need to support clip too. That's cap only for now. + // s.active_model_panel != PointModel::Clip + + std::vector objectPoints; + objectPoints.push_back(cv::Point3f(s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right + objectPoints.push_back(cv::Point3f(-s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left + objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top + + //Bitmap origin is top left + std::vector trackedPoints; + // Stuff bitmap point in there making sure they match the order of the object point + // Find top most point, that's the one with min Y as we assume our guy's head is not up side down + + int minY = std::numeric_limits::max(); + for (int i = 0; i < 3; i++) + { + if (iImagePoints[i][1] maxX) + { + maxX = iImagePoints[i][0]; + rightPointIndex = i; + } + } + + // Find left most point + int leftPointIndex = -1; + for (int i = 0; i < 3; i++) + { + // Excluding top most point + if (i != topPointIndex && i != rightPointIndex) + { + leftPointIndex = i; + break; + } + } + + // + trackedPoints.push_back(cv::Point2f(iImagePoints[rightPointIndex][0], iImagePoints[rightPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iImagePoints[leftPointIndex][0], iImagePoints[leftPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iImagePoints[topPointIndex][0], iImagePoints[topPointIndex][1])); + + std::cout << "Object: " << objectPoints << "\n"; + std::cout << "Points: " << trackedPoints << "\n"; + + + // Create our camera matrix + // TODO: Just do that once, use data member instead + // Double or Float? + cv::Mat cameraMatrix; + cameraMatrix.create(3, 3, CV_64FC1); + cameraMatrix.setTo(cv::Scalar(0)); + cameraMatrix.at(0, 0) = camera->info.focalLengthX; + cameraMatrix.at(1, 1) = camera->info.focalLengthY; + cameraMatrix.at(0, 2) = camera->info.principalPointX; + cameraMatrix.at(1, 2) = camera->info.principalPointY; + cameraMatrix.at(2, 2) = 1; + + // Create distortion cooefficients + cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1); + // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6 + distCoeffs.at(0, 0) = 0; // Radial first order + distCoeffs.at(1, 0) = camera->info.radialDistortionSecondOrder; // Radial second order + distCoeffs.at(2, 0) = 0; // Tangential first order + distCoeffs.at(3, 0) = 0; // Tangential second order + distCoeffs.at(4, 0) = 0; // Radial third order + distCoeffs.at(5, 0) = camera->info.radialDistortionFourthOrder; // Radial fourth order + distCoeffs.at(6, 0) = 0; // Radial fith order + distCoeffs.at(7, 0) = camera->info.radialDistortionSixthOrder; // Radial sixth order + + // Define our solution arrays + // They will receive up to 4 solutions for our P3P problem + + + // TODO: try SOLVEPNP_AP3P too + iAngles.clear(); + iBestSolutionIndex = -1; + int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P); + + if (solutionCount > 0) + { + std::cout << "Solution count: " << solutionCount << "\n"; + int minPitch = std::numeric_limits::max(); + // Find the solution we want + for (int i = 0; i < solutionCount; i++) + { + std::cout << "Translation:\n"; + std::cout << iTranslations.at(i); + std::cout << "\n"; + std::cout << "Rotation:\n"; + //std::cout << rvecs.at(i); + cv::Mat rotationCameraMatrix; + cv::Rodrigues(iRotations[i], rotationCameraMatrix); + cv::Vec3d angles; + getEulerAngles(rotationCameraMatrix,angles); + iAngles.push_back(angles); + + // Check if pitch is closest to zero + int absolutePitch = std::abs(angles[0]); + if (minPitch > absolutePitch) + { + minPitch = absolutePitch; + iBestSolutionIndex = i; + } + + //cv::Vec3f angles=EulerAngles(quaternion); + std::cout << angles; + std::cout << "\n"; + } + + std::cout << "\n"; + + } + + } + + // Send solution data back to main thread + QMutexLocker l2(&data_lock); + if (iBestSolutionIndex != -1) + { + iBestAngles = iAngles[iBestSolutionIndex]; + iBestTranslation = iTranslations[iBestSolutionIndex]; + } + + } + + if (preview_visible) + { + if (topPointIndex != -1) + { + // Render a cross to indicate which point is the head + preview_frame->draw_head_center(points[topPointIndex][0], points[topPointIndex][1]); + } + + widget->update_image(preview_frame->get_bitmap()); + + auto [ w, h ] = widget->preview_size(); + if (w != preview_width || h != preview_height) + { + preview_width = w; preview_height = h; + preview_frame = traits->make_preview(w, h); + } + } + } + } +} + +bool EasyTracker::maybe_reopen_camera() +{ + QMutexLocker l(&camera_mtx); + + return camera->start(s.camera_name, + s.cam_fps, s.cam_res_x, s.cam_res_y); +} + +void EasyTracker::set_fov(int value) +{ + QMutexLocker l(&camera_mtx); + camera->set_fov(value); +} + +module_status EasyTracker::start_tracker(QFrame* video_frame) +{ + //video_frame->setAttribute(Qt::WA_NativeWindow); + + widget = std::make_unique(video_frame); + layout = std::make_unique(video_frame); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(widget.get()); + video_frame->setLayout(layout.get()); + //video_widget->resize(video_frame->width(), video_frame->height()); + video_frame->show(); + + start(QThread::HighPriority); + + return {}; +} + +void EasyTracker::data(double *data) +{ + if (ever_success.load(std::memory_order_relaxed)) + { + // Get data back from tracker thread + QMutexLocker l(&data_lock); + data[Yaw] = iBestAngles[1]; + data[Pitch] = iBestAngles[0]; + data[Roll] = iBestAngles[2]; + data[TX] = iBestTranslation[0]; + data[TY] = iBestTranslation[1]; + data[TZ] = iBestTranslation[2]; + } +} + +bool EasyTracker::center() +{ + QMutexLocker l(¢er_lock); + //TODO: Do we need to do anything there? + return false; +} + +int EasyTracker::get_n_points() +{ + return (int)point_count.load(std::memory_order_relaxed); +} + +bool EasyTracker::get_cam_info(pt_camera_info& info) +{ + QMutexLocker l(&camera_mtx); + bool ret; + + std::tie(ret, info) = camera->get_info(); + return ret; +} + + +} // ns pt_impl diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h new file mode 100644 index 00000000..fe99ba85 --- /dev/null +++ b/tracker-easy/tracker-easy.h @@ -0,0 +1,92 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * + * 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. + */ + +#pragma once + +#include "api/plugin-api.hpp" +#include "tracker-easy-api.h" +#include "cv/numeric.hpp" +#include "video/video-widget.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace pt_impl { + +class EasyTrackerDialog; + +using namespace numeric_types; + +struct EasyTracker : QThread, ITracker +{ + friend class EasyTrackerDialog; + + template using pointer = pt_pointer; + + explicit EasyTracker(pointer const& pt_runtime_traits); + ~EasyTracker() override; + module_status start_tracker(QFrame* parent_window) override; + void data(double* data) override; + bool center() override; + + int get_n_points(); + [[nodiscard]] bool get_cam_info(pt_camera_info& info); + +private: + void run() override; + + bool maybe_reopen_camera(); + void set_fov(int value); + + pointer traits; + + QMutex camera_mtx; + + + pt_settings s; + + std::unique_ptr layout; + std::vector points; + std::vector iImagePoints; + + int preview_width = 320, preview_height = 240; + + pointer point_extractor; + pointer camera; + pointer widget; + pointer frame; + pointer preview_frame; + + std::atomic point_count { 0 }; + std::atomic ever_success = false; + mutable QMutex center_lock, data_lock; + + // Translation solutions + std::vector iTranslations; + // Rotation solutions + std::vector iRotations; + // Angle solutions, pitch, yaw, roll, in this order + std::vector iAngles; + // The index of our best solution in the above arrays + int iBestSolutionIndex = -1; + // Best translation + cv::Vec3d iBestTranslation; + // Best angles + cv::Vec3d iBestAngles; +}; + +} // ns pt_impl + +using Tracker_PT = pt_impl::EasyTracker; -- cgit v1.2.3