diff options
author | Stanislaw Halik <sthalik@misaki.pl> | 2015-10-30 07:37:41 +0100 |
---|---|---|
committer | Stanislaw Halik <sthalik@misaki.pl> | 2015-10-30 08:39:32 +0100 |
commit | aa066bdd4622d4f6824fee864f6be6806813f04d (patch) | |
tree | 3df328b8b364cba2373a85827191b259bd78d546 /ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | |
parent | d6a54431d178632a2bf466c9904f74abd143afe6 (diff) |
move to subdirectory-based build system
Closes #224
Diffstat (limited to 'ftnoir_tracker_pt/ftnoir_tracker_pt.cpp')
-rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 219 |
1 files changed, 0 insertions, 219 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp deleted file mode 100644 index 15a60962..00000000 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2015 Stanislaw Halik <sthalik@misaki.pl> - * - * 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 <QHBoxLayout> -#include <cmath> -#include <QDebug> -#include <QFile> -#include <QCoreApplication> -#include "opentrack/camera-names.hpp" - -//#define PT_PERF_LOG //log performance - -//----------------------------------------------------------------------------- -Tracker_PT::Tracker_PT() - : mutex(QMutex::Recursive), - commands(0), - video_widget(NULL), - video_frame(NULL), - ever_success(false) -{ - connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings())); -} - -Tracker_PT::~Tracker_PT() -{ - set_command(ABORT); - wait(); - delete video_widget; - video_widget = NULL; - if (video_frame->layout()) delete video_frame->layout(); - camera.stop(); -} - -void Tracker_PT::set_command(Command command) -{ - //QMutexLocker lock(&mutex); - commands |= command; -} - -void Tracker_PT::reset_command(Command command) -{ - //QMutexLocker lock(&mutex); - commands &= ~command; -} - -bool Tracker_PT::get_focal_length(float& ret) -{ - QMutexLocker l(&camera_mtx); - CamInfo info; - const bool res = camera.get_info(info); - if (res) - { - static constexpr double pi = 3.14159265359; - const int w = info.res_x, h = info.res_y; - const double diag = sqrt(1. + h/(double)w * h/(double)w); - const double diag_fov = static_cast<int>(s.fov) * pi / 180.; - const double fov = 2.*atan(tan(diag_fov/2.0)/diag); - ret = .5 / tan(.5 * fov); - return true; - } - return false; -} - -void Tracker_PT::run() -{ -#ifdef PT_PERF_LOG - QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt"); - if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return; - QTextStream log_stream(&log_file); -#endif - - apply_settings(); - - while((commands & ABORT) == 0) - { - const double dt = time.elapsed() * 1e-9; - time.start(); - cv::Mat frame; - bool new_frame; - - { - QMutexLocker l(&camera_mtx); - new_frame = camera.get_frame(dt, &frame); - } - - if (new_frame && !frame.empty()) - { - QMutexLocker lock(&mutex); - - std::vector<cv::Vec2f> points = point_extractor.extract_points(frame); - - // blobs are sorted in order of circularity - if (points.size() > PointModel::N_POINTS) - points.resize(PointModel::N_POINTS); - - bool success = points.size() == PointModel::N_POINTS; - - float fx; - if (!get_focal_length(fx)) - continue; - - if (success) - { - point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout); - } - - Affine X_CM = pose(); - - ever_success |= success; - - { - Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below - Affine X_GH = X_CM * X_MH; - cv::Vec3f p = X_GH.t; // head (center?) position in global space - cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen - points.push_back(p_); - } - - for (unsigned i = 0; i < points.size(); i++) - { - auto& p = points[i]; - auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2); - cv::Scalar color(0, 255, 0); - if (i == points.size()-1) - color = cv::Scalar(0, 0, 255); - cv::line(frame, - cv::Point(p2.x - 20, p2.y), - cv::Point(p2.x + 20, p2.y), - color, - 4); - cv::line(frame, - cv::Point(p2.x, p2.y - 20), - cv::Point(p2.x, p2.y + 20), - color, - 4); - } - - video_widget->update_image(frame); - } -#ifdef PT_PERF_LOG - log_stream<<"dt: "<<dt; - if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps; - log_stream<<"\n"; -#endif - } - qDebug()<<"Tracker:: Thread stopping"; -} - -void Tracker_PT::apply_settings() -{ - qDebug()<<"Tracker:: Applying settings"; - QMutexLocker l(&camera_mtx); - camera.stop(); - camera.set_device_index(camera_name_to_index(s.camera_name)); - camera.set_res(s.cam_res_x, s.cam_res_y); - camera.set_fps(s.cam_fps); - camera.start(); - qDebug()<<"Tracker::apply ends"; -} - -void Tracker_PT::start_tracker(QFrame *parent_window) -{ - this->video_frame = parent_window; - video_frame->setAttribute(Qt::WA_NativeWindow); - video_frame->show(); - video_widget = new PTVideoWidget(video_frame); - QHBoxLayout* video_layout = new QHBoxLayout(parent_window); - video_layout->setContentsMargins(0, 0, 0, 0); - video_layout->addWidget(video_widget); - video_frame->setLayout(video_layout); - video_widget->resize(video_frame->width(), video_frame->height()); - start(); -} - -void Tracker_PT::data(double *data) -{ - if (ever_success) - { - Affine X_CM = pose(); - - Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); - Affine X_GH = X_CM * X_MH; - - cv::Matx33f R = X_GH.R; - cv::Vec3f t = X_GH.t; - - // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame - // -z -> x, y -> z, x -> -y - cv::Matx33f R_EG(0, 0,-1, - -1, 0, 0, - 0, 1, 0); - R = R_EG * R * R_EG.t(); - - // extract rotation angles - float alpha, beta, gamma; - beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) ); - alpha = atan2( R(1,0), R(0,0)); - gamma = atan2( R(2,1), R(2,2)); - - // extract rotation angles - data[Yaw] = rad2deg * alpha; - data[Pitch] = -rad2deg * beta; - data[Roll] = rad2deg * gamma; - // get translation(s) - data[TX] = t[0] / 10.0; // convert to cm - data[TY] = t[1] / 10.0; - data[TZ] = t[2] / 10.0; - } -} - -#include "ftnoir_tracker_pt_dialog.h" -OPENTRACK_DECLARE_TRACKER(Tracker_PT, TrackerDialog_PT, TrackerDll) - |