diff options
Diffstat (limited to 'tracker-ht/ftnoir_tracker_ht.cpp')
| -rw-r--r-- | tracker-ht/ftnoir_tracker_ht.cpp | 225 |
1 files changed, 225 insertions, 0 deletions
diff --git a/tracker-ht/ftnoir_tracker_ht.cpp b/tracker-ht/ftnoir_tracker_ht.cpp new file mode 100644 index 00000000..cc9d2ba1 --- /dev/null +++ b/tracker-ht/ftnoir_tracker_ht.cpp @@ -0,0 +1,225 @@ +#include "headtracker-ftnoir.h" +#include "ftnoir_tracker_ht.h" +#include "ui_ht-trackercontrols.h" +#include "opentrack/plugin-api.hpp" +#include <cmath> +#include "opentrack/camera-names.hpp" +#include "opentrack-compat/sleep.hpp" + +typedef struct { + int width; + int height; +} resolution_tuple; + +static resolution_tuple resolution_choices[] = { + { 640, 480 }, + { 320, 240 }, + { 320, 200 }, + { 0, 0 } +}; + +void Tracker::load_settings(ht_config_t* config) +{ + int nframes = 0; + switch (static_cast<int>(s.fps)) + { + default: + case 0: + nframes = 0; + break; + case 1: + nframes = 30; + break; + case 2: + nframes = 60; + break; + case 3: + nframes = 120; + break; + case 4: + nframes = 180; + break; + } + + config->classification_delay = 500; + config->field_of_view = s.fov; + config->max_keypoints = 150; + config->keypoint_distance = 3.5; + config->force_fps = nframes; + config->camera_index = camera_name_to_index(s.camera_name); + + config->ransac_max_reprojection_error = 25; + config->ransac_max_inlier_error = config->ransac_max_reprojection_error; + + config->pyrlk_pyramids = 0; + config->pyrlk_win_size_w = config->pyrlk_win_size_h = 21; + + config->ransac_max_mean_error = 999; + config->ransac_abs_max_mean_error = 999; + + config->debug = 1; + config->ransac_min_features = 0.95; + config->ransac_num_iters = 300; + + int res = s.resolution; + if (res < 0 || res >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple))) + res = 0; + resolution_tuple r = resolution_choices[res]; + config->force_width = r.width; + config->force_height = r.height; + config->flandmark_delay = 50; + for (int i = 0; i < 5; i++) + config->dist_coeffs[i] = 0; +} + +Tracker::Tracker() : + ht(nullptr), + ypr {0,0,0, 0,0,0}, + videoWidget(nullptr), + layout(nullptr), + should_stop(false) +{ +} + +Tracker::~Tracker() +{ + should_stop = true; + wait(); + ht_free_context(ht); + if (layout) + delete layout; + if (videoWidget) + delete videoWidget; +} + +void Tracker::start_tracker(QFrame* videoframe) +{ + videoframe->show(); + videoWidget = new HTVideoWidget(videoframe); + QHBoxLayout* layout = new QHBoxLayout(); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(videoWidget); + if (videoframe->layout()) + delete videoframe->layout(); + videoframe->setLayout(layout); + videoWidget->show(); + this->layout = layout; + + load_settings(&conf); + ht = ht_make_context(&conf, nullptr); + start(); +} + +void Tracker::run() +{ + while (!should_stop) + { + ht_result_t euler; + euler.filled = false; + { + QMutexLocker l(&camera_mtx); + + if (!ht_cycle(ht, &euler)) + break; + } + if (euler.filled) + { + QMutexLocker l(&ypr_mtx); + ypr[TX] = euler.tx; + ypr[TY] = euler.ty; + ypr[TZ] = euler.tz; + ypr[Yaw] = euler.rotx; + ypr[Pitch] = euler.roty; + ypr[Roll] = euler.rotz; + } + { + const cv::Mat frame_ = ht_get_bgr_frame(ht); + if (frame_.cols <= HT_MAX_VIDEO_WIDTH && frame_.rows <= HT_MAX_VIDEO_HEIGHT && frame_.channels() <= HT_MAX_VIDEO_CHANNELS) + { + QMutexLocker l(&frame_mtx); + + const int cols = frame_.cols; + const int rows = frame_.rows; + const int pitch = cols * 3; + for (int y = 0; y < rows; y++) + { + for (int x = 0; x < cols; x++) + { + unsigned char* dest = &frame.frame[y * pitch + 3 * x]; + const cv::Vec3b& elt = frame_.at<cv::Vec3b>(y, x); + const cv::Scalar elt2 = static_cast<cv::Scalar>(elt); + dest[0] = elt2.val[0]; + dest[1] = elt2.val[1]; + dest[2] = elt2.val[2]; + } + } + frame.channels = frame_.channels(); + frame.width = frame_.cols; + frame.height = frame_.rows; + } + } + } + // give opencv time to exit camera threads, etc. + portable::sleep(500); +} + +void Tracker::data(double* data) +{ + { + QMutexLocker l(&frame_mtx); + + if (frame.width > 0) + { + videoWidget->update_image(frame.frame, frame.width, frame.height); + frame.width = 0; + } + } + + { + QMutexLocker l(&ypr_mtx); + + for (int i = 0; i < 6; i++) + data[i] = ypr[i]; + } +} + +TrackerControls::TrackerControls() : tracker(nullptr) +{ + ui.setupUi(this); + ui.cameraName->clear(); + QList<QString> names = get_camera_names(); + names.prepend("Any available"); + ui.cameraName->addItems(names); + tie_setting(s.camera_name, ui.cameraName); + tie_setting(s.fps, ui.cameraFPS); + tie_setting(s.fov, ui.cameraFOV); + tie_setting(s.resolution, ui.resolution); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.camera_settings, SIGNAL(pressed()), this, SLOT(camera_settings())); +} + +void TrackerControls::doOK() +{ + s.b->save(); + this->close(); +} + +void TrackerControls::doCancel() +{ + s.b->reload(); + this->close(); +} + +void TrackerControls::camera_settings() +{ + if (tracker) + { + cv::VideoCapture* cap = ht_capture(tracker->ht); + open_camera_settings(cap, s.camera_name, &tracker->camera_mtx); + } + else + open_camera_settings(nullptr, s.camera_name, nullptr); +} + +OPENTRACK_DECLARE_TRACKER(Tracker, TrackerControls, TrackerDll) |