From c58c0af311892929dbce4e5437c4035214552438 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 14 Sep 2013 15:34:44 +0200 Subject: Run dos2unix on the tree. No user-facing changes. --- ftnoir_tracker_pt/camera.cpp | 426 +++++++------- ftnoir_tracker_pt/camera.h | 238 ++++---- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 522 ++++++++--------- ftnoir_tracker_pt/ftnoir_tracker_pt.h | 192 +++---- ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 674 +++++++++++----------- ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h | 204 +++---- ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp | 80 +-- ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h | 36 +- ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp | 298 +++++----- ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 166 +++--- ftnoir_tracker_pt/point_extractor.cpp | 192 +++---- ftnoir_tracker_pt/point_extractor.h | 62 +- ftnoir_tracker_pt/point_tracker.cpp | 704 +++++++++++------------ ftnoir_tracker_pt/point_tracker.h | 228 ++++---- ftnoir_tracker_pt/resource.h | 28 +- ftnoir_tracker_pt/timer.cpp | 128 ++--- ftnoir_tracker_pt/timer.h | 86 +-- ftnoir_tracker_pt/trans_calib.cpp | 86 +-- ftnoir_tracker_pt/trans_calib.h | 76 +-- ftnoir_tracker_pt/video_widget.cpp | 104 ++-- ftnoir_tracker_pt/video_widget.h | 84 +-- 21 files changed, 2307 insertions(+), 2307 deletions(-) (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp index fe39b436..46dd20c5 100644 --- a/ftnoir_tracker_pt/camera.cpp +++ b/ftnoir_tracker_pt/camera.cpp @@ -1,213 +1,213 @@ -/* 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. - */ - -#include "camera.h" -#include - -using namespace cv; - -// ---------------------------------------------------------------------------- -void Camera::set_index(int index) -{ - if (desired_index != index) - { - desired_index = index; - _set_index(); - - // reset fps - dt_valid = 0; - dt_mean = 0; - active_index = index; - } -} - -void Camera::set_f(float f) -{ - if (cam_desired.f != f) - { - cam_desired.f = f; - _set_f(); - } -} -void Camera::set_fps(int fps) -{ - if (cam_desired.fps != fps) - { - cam_desired.fps = fps; - _set_fps(); - } -} - -void Camera::set_res(int x_res, int y_res) -{ - if (cam_desired.res_x != x_res || cam_desired.res_y != y_res) - { - cam_desired.res_x = x_res; - cam_desired.res_y = y_res; - _set_res(); - } -} - -bool Camera::get_frame(float dt, cv::Mat* frame) -{ - bool new_frame = _get_frame(frame); - // measure fps of valid frames - const float dt_smoothing_const = 0.9; - dt_valid += dt; - if (new_frame) - { - dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid; - cam_info.fps = 1.0 / dt_mean; - dt_valid = 0; - } - return new_frame; -} - -// ---------------------------------------------------------------------------- -void CVCamera::start() -{ - cap = new VideoCapture(desired_index); -// extract camera info - active = true; - active_index = desired_index; - cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); - cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); -} - -void CVCamera::stop() -{ - if (cap) { - cap->release(); - delete cap; - cap = NULL; - } - active = false; -} - -bool CVCamera::_get_frame(Mat* frame) -{ - bool ret = cap->read(*frame); - //if (ret) - // flip(tmp, *frame, 0); - return ret; -} - -void CVCamera::_set_index() -{ - if (active) restart(); -} - -void CVCamera::_set_f() -{ - cam_info.f = cam_desired.f; -} - -void CVCamera::_set_fps() -{ - if (cap) cap->set(CV_CAP_PROP_FPS, cam_desired.fps); -} - -void CVCamera::_set_res() -{ - if (cap) - { - cap->set(CV_CAP_PROP_FRAME_WIDTH, cam_desired.res_x); - cap->set(CV_CAP_PROP_FRAME_HEIGHT, cam_desired.res_y); - cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); - cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); - } -} - -#if 0 -// ---------------------------------------------------------------------------- -VICamera::VICamera() : frame_buffer(NULL) -{ - VI.listDevices(); -} - -void VICamera::start() -{ - if (desired_index >= 0) - { - - - if (cam_desired.res_x == 0 || cam_desired.res_y == 0) - VI.setupDevice(desired_index); - else - VI.setupDevice(desired_index, cam_desired.res_x, cam_desired.res_y); - - active = true; - active_index = desired_index; - - cam_info.res_x = VI.getWidth(active_index); - cam_info.res_y = VI.getHeight(active_index); - new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3); - // If matrix is not continuous we have to copy manually via frame_buffer - if (!new_frame.isContinuous()) { - unsigned int size = VI.getSize(active_index); - frame_buffer = new unsigned char[size]; - } - } -} - -void VICamera::stop() -{ - if (active) - { - VI.stopDevice(active_index); - } - if (frame_buffer) - { - delete[] frame_buffer; - frame_buffer = NULL; - } - active = false; -} - -bool VICamera::_get_frame(Mat* frame) -{ - if (active && VI.isFrameNew(active_index)) - { - if (new_frame.isContinuous()) - { - VI.getPixels(active_index, new_frame.data, false, true); - } - else - { - // If matrix is not continuous we have to copy manually via frame_buffer - VI.getPixels(active_index, frame_buffer, false, true); - new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3, frame_buffer).clone(); - } - *frame = new_frame; - return true; - } - return false; -} - -void VICamera::_set_index() -{ - if (active) restart(); -} - -void VICamera::_set_f() -{ - cam_info.f = cam_desired.f; -} - -void VICamera::_set_fps() -{ - bool was_active = active; - if (active) stop(); - VI.setIdealFramerate(desired_index, cam_desired.fps); - if (was_active) start(); -} - -void VICamera::_set_res() -{ - if (active) restart(); -} -#endif +/* 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. + */ + +#include "camera.h" +#include + +using namespace cv; + +// ---------------------------------------------------------------------------- +void Camera::set_index(int index) +{ + if (desired_index != index) + { + desired_index = index; + _set_index(); + + // reset fps + dt_valid = 0; + dt_mean = 0; + active_index = index; + } +} + +void Camera::set_f(float f) +{ + if (cam_desired.f != f) + { + cam_desired.f = f; + _set_f(); + } +} +void Camera::set_fps(int fps) +{ + if (cam_desired.fps != fps) + { + cam_desired.fps = fps; + _set_fps(); + } +} + +void Camera::set_res(int x_res, int y_res) +{ + if (cam_desired.res_x != x_res || cam_desired.res_y != y_res) + { + cam_desired.res_x = x_res; + cam_desired.res_y = y_res; + _set_res(); + } +} + +bool Camera::get_frame(float dt, cv::Mat* frame) +{ + bool new_frame = _get_frame(frame); + // measure fps of valid frames + const float dt_smoothing_const = 0.9; + dt_valid += dt; + if (new_frame) + { + dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid; + cam_info.fps = 1.0 / dt_mean; + dt_valid = 0; + } + return new_frame; +} + +// ---------------------------------------------------------------------------- +void CVCamera::start() +{ + cap = new VideoCapture(desired_index); +// extract camera info + active = true; + active_index = desired_index; + cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); + cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); +} + +void CVCamera::stop() +{ + if (cap) { + cap->release(); + delete cap; + cap = NULL; + } + active = false; +} + +bool CVCamera::_get_frame(Mat* frame) +{ + bool ret = cap->read(*frame); + //if (ret) + // flip(tmp, *frame, 0); + return ret; +} + +void CVCamera::_set_index() +{ + if (active) restart(); +} + +void CVCamera::_set_f() +{ + cam_info.f = cam_desired.f; +} + +void CVCamera::_set_fps() +{ + if (cap) cap->set(CV_CAP_PROP_FPS, cam_desired.fps); +} + +void CVCamera::_set_res() +{ + if (cap) + { + cap->set(CV_CAP_PROP_FRAME_WIDTH, cam_desired.res_x); + cap->set(CV_CAP_PROP_FRAME_HEIGHT, cam_desired.res_y); + cam_info.res_x = cap->get(CV_CAP_PROP_FRAME_WIDTH); + cam_info.res_y = cap->get(CV_CAP_PROP_FRAME_HEIGHT); + } +} + +#if 0 +// ---------------------------------------------------------------------------- +VICamera::VICamera() : frame_buffer(NULL) +{ + VI.listDevices(); +} + +void VICamera::start() +{ + if (desired_index >= 0) + { + + + if (cam_desired.res_x == 0 || cam_desired.res_y == 0) + VI.setupDevice(desired_index); + else + VI.setupDevice(desired_index, cam_desired.res_x, cam_desired.res_y); + + active = true; + active_index = desired_index; + + cam_info.res_x = VI.getWidth(active_index); + cam_info.res_y = VI.getHeight(active_index); + new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3); + // If matrix is not continuous we have to copy manually via frame_buffer + if (!new_frame.isContinuous()) { + unsigned int size = VI.getSize(active_index); + frame_buffer = new unsigned char[size]; + } + } +} + +void VICamera::stop() +{ + if (active) + { + VI.stopDevice(active_index); + } + if (frame_buffer) + { + delete[] frame_buffer; + frame_buffer = NULL; + } + active = false; +} + +bool VICamera::_get_frame(Mat* frame) +{ + if (active && VI.isFrameNew(active_index)) + { + if (new_frame.isContinuous()) + { + VI.getPixels(active_index, new_frame.data, false, true); + } + else + { + // If matrix is not continuous we have to copy manually via frame_buffer + VI.getPixels(active_index, frame_buffer, false, true); + new_frame = cv::Mat(cam_info.res_y, cam_info.res_x, CV_8UC3, frame_buffer).clone(); + } + *frame = new_frame; + return true; + } + return false; +} + +void VICamera::_set_index() +{ + if (active) restart(); +} + +void VICamera::_set_f() +{ + cam_info.f = cam_desired.f; +} + +void VICamera::_set_fps() +{ + bool was_active = active; + if (active) stop(); + VI.setIdealFramerate(desired_index, cam_desired.fps); + if (was_active) start(); +} + +void VICamera::_set_res() +{ + if (active) restart(); +} +#endif diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h index 6fec12da..5f1f56b0 100644 --- a/ftnoir_tracker_pt/camera.h +++ b/ftnoir_tracker_pt/camera.h @@ -1,119 +1,119 @@ -/* 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. - */ - -#ifndef CAMERA_H -#define CAMERA_H - -#include -#include -//#include "videoInput/videoInput.h" - -// ---------------------------------------------------------------------------- -struct CamInfo -{ - CamInfo() : res_x(0), res_y(0), fps(0), f(1) {} - - int res_x; - int res_y; - int fps; - float f; // (focal length) / (sensor width) -}; - -// ---------------------------------------------------------------------------- -// base class for cameras -class Camera -{ -public: - Camera() : desired_index(0), active_index(-1), active(false), dt_valid(0), dt_mean(0) {} - virtual ~Camera() {} - - // start/stop capturing - virtual void start() = 0; - virtual void stop() = 0; - void restart() { stop(); start(); } - - void set_index(int index); - void set_f(float f); - void set_fps(int fps); - void set_res(int x_res, int y_res); - - // gets a frame from the camera, dt: time since last call in seconds - bool get_frame(float dt, cv::Mat* frame); - - // WARNING: returned references are valid as long as object - const CamInfo& get_info() const { return cam_info; } - const CamInfo& get_desired() const { return cam_desired; } - -protected: - // get a frame from the camera - virtual bool _get_frame(cv::Mat* frame) = 0; - - // update the camera - virtual void _set_index() = 0; - virtual void _set_f() = 0; - virtual void _set_fps() = 0; - virtual void _set_res() = 0; - - int desired_index; - int active_index; - bool active; - float dt_valid; - float dt_mean; - CamInfo cam_info; - CamInfo cam_desired; -}; - - -// ---------------------------------------------------------------------------- -// OpenCV camera - -class CVCamera : public Camera -{ -public: - CVCamera() : cap(NULL) {} - ~CVCamera() { stop(); } - - void start(); - void stop(); - -protected: - bool _get_frame(cv::Mat* frame); - void _set_index(); - void _set_f(); - void _set_fps(); - void _set_res(); - - cv::VideoCapture* cap; -}; - - -// ---------------------------------------------------------------------------- -// videoInput camera -#if 0 -class VICamera : public Camera -{ -public: - VICamera(); - ~VICamera() { stop(); } - - void start(); - void stop(); - -protected: - bool _get_frame(cv::Mat* frame); - void _set_index(); - void _set_f(); - void _set_fps(); - void _set_res(); - - videoInput VI; - cv::Mat new_frame; - unsigned char* frame_buffer; -}; -#endif - -#endif //CAMERA_H +/* 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. + */ + +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include +//#include "videoInput/videoInput.h" + +// ---------------------------------------------------------------------------- +struct CamInfo +{ + CamInfo() : res_x(0), res_y(0), fps(0), f(1) {} + + int res_x; + int res_y; + int fps; + float f; // (focal length) / (sensor width) +}; + +// ---------------------------------------------------------------------------- +// base class for cameras +class Camera +{ +public: + Camera() : desired_index(0), active_index(-1), active(false), dt_valid(0), dt_mean(0) {} + virtual ~Camera() {} + + // start/stop capturing + virtual void start() = 0; + virtual void stop() = 0; + void restart() { stop(); start(); } + + void set_index(int index); + void set_f(float f); + void set_fps(int fps); + void set_res(int x_res, int y_res); + + // gets a frame from the camera, dt: time since last call in seconds + bool get_frame(float dt, cv::Mat* frame); + + // WARNING: returned references are valid as long as object + const CamInfo& get_info() const { return cam_info; } + const CamInfo& get_desired() const { return cam_desired; } + +protected: + // get a frame from the camera + virtual bool _get_frame(cv::Mat* frame) = 0; + + // update the camera + virtual void _set_index() = 0; + virtual void _set_f() = 0; + virtual void _set_fps() = 0; + virtual void _set_res() = 0; + + int desired_index; + int active_index; + bool active; + float dt_valid; + float dt_mean; + CamInfo cam_info; + CamInfo cam_desired; +}; + + +// ---------------------------------------------------------------------------- +// OpenCV camera + +class CVCamera : public Camera +{ +public: + CVCamera() : cap(NULL) {} + ~CVCamera() { stop(); } + + void start(); + void stop(); + +protected: + bool _get_frame(cv::Mat* frame); + void _set_index(); + void _set_f(); + void _set_fps(); + void _set_res(); + + cv::VideoCapture* cap; +}; + + +// ---------------------------------------------------------------------------- +// videoInput camera +#if 0 +class VICamera : public Camera +{ +public: + VICamera(); + ~VICamera() { stop(); } + + void start(); + void stop(); + +protected: + bool _get_frame(cv::Mat* frame); + void _set_index(); + void _set_f(); + void _set_fps(); + void _set_res(); + + videoInput VI; + cv::Mat new_frame; + unsigned char* frame_buffer; +}; +#endif + +#endif //CAMERA_H diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 99b2f680..48702d19 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -1,261 +1,261 @@ -/* 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. - */ - -#include "ftnoir_tracker_pt.h" -#include -#include -#include -#include -#include -#include "facetracknoir/global-settings.h" - -using namespace std; -using namespace cv; - -//#define PT_PERF_LOG //log performance - -//----------------------------------------------------------------------------- -Tracker::Tracker() - : tracking_valid(false), frame_count(0), commands(0), video_widget(NULL), fresh(false) -{ - should_quit = false; - qDebug()<<"Tracker::Tracker"; - TrackerSettings settings; - settings.load_ini(); - apply(settings); -} - -Tracker::~Tracker() -{ - qDebug()<<"Tracker::~Tracker"; - set_command(ABORT); - wait(); - if (video_widget) delete video_widget; -} - -void Tracker::set_command(Command command) -{ - //QMutexLocker lock(&mutex); - commands |= command; -} - -void Tracker::reset_command(Command command) -{ - //QMutexLocker lock(&mutex); - commands &= ~command; -} - -void Tracker::run() -{ - qDebug()<<"Tracker:: Thread started"; - -#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 - - time.start(); - float dt; - bool new_frame; - forever - { - { - QMutexLocker lock(&mutex); - if (should_quit) - break; - - if (commands & ABORT) break; - if (commands & PAUSE) continue; - commands = 0; - - dt = time.elapsed() / 1000.0; - time.restart(); - - new_frame = camera.get_frame(dt, &frame); - if (new_frame && !frame.empty()) - { - const std::vector& points = point_extractor.extract_points(frame, dt, draw_frame); - tracking_valid = point_tracker.track(points, camera.get_info().f, dt); - frame_count++; - fresh = true; - } -#ifdef PT_PERF_LOG - log_stream<<"dt: "<(new PointModel(settings.M01, settings.M02)); - point_tracker.dynamic_pose_resolution = settings.dyn_pose_res; - sleep_time = settings.sleep_time; - point_tracker.dt_reset = settings.reset_time / 1000.0; - draw_frame = settings.video_widget; - cam_pitch = settings.cam_pitch; - - bEnableRoll = settings.bEnableRoll; - bEnablePitch = settings.bEnablePitch; - bEnableYaw = settings.bEnableYaw; - bEnableX = settings.bEnableX; - bEnableY = settings.bEnableY; - bEnableZ = settings.bEnableZ; - - t_MH = settings.t_MH; - qDebug()<<"Tracker::apply ends"; -} - -void Tracker::reset() -{ - QMutexLocker lock(&mutex); - point_tracker.reset(); -} - -void Tracker::center() -{ - point_tracker.reset(); // we also do a reset here since there is no reset shortkey yet - QMutexLocker lock(&mutex); - FrameTrafo X_CM_0 = point_tracker.get_pose(); - FrameTrafo X_MH(Matx33f::eye(), t_MH); - X_CH_0 = X_CM_0 * X_MH; -} - -void Tracker::refreshVideo() -{ - if (video_widget && fresh) - { - Mat frame_copy; - std::auto_ptr< vector > points; - { - //QMutexLocker lock(&mutex); - if (!draw_frame || frame.empty()) return; - - // copy the frame and points from the tracker thread - frame_copy = frame.clone(); - points = std::auto_ptr< vector >(new vector(point_extractor.get_points())); - } - //QMutexLocker lck(&mutex); - video_widget->update_image(frame_copy, points); - } -} - -void Tracker::StartTracker(QFrame* videoframe) -{ - const int VIDEO_FRAME_WIDTH = videoframe->width(); - const int VIDEO_FRAME_HEIGHT = videoframe->height(); - TrackerSettings settings; - settings.load_ini(); - apply(settings); - qDebug("Tracker::Initialize"); - // setup video frame - video_widget = new VideoWidget(videoframe); - QHBoxLayout* layout = new QHBoxLayout(); - layout->setContentsMargins(0, 0, 0, 0); - layout->addWidget(video_widget); - if (videoframe->layout()) delete videoframe->layout(); - videoframe->setLayout(layout); - video_widget->resize(VIDEO_FRAME_WIDTH, VIDEO_FRAME_HEIGHT); - videoframe->show(); - connect(&timer, SIGNAL(timeout()), this, SLOT(paint_widget())); - timer.start(40); - camera.start(); - start(); - reset_command(PAUSE); -} - -void Tracker::paint_widget() { - if (fresh) { - fresh = false; - video_widget->update(); - } -} - -bool Tracker::GiveHeadPoseData(double *data) -{ - const double rad2deg = 180.0/3.14159265; - const double deg2rad = 1.0/rad2deg; - { - QMutexLocker lock(&mutex); - - if (!tracking_valid) return false; - - FrameTrafo X_CM = point_tracker.get_pose(); - FrameTrafo X_MH(Matx33f::eye(), t_MH); - FrameTrafo X_CH = X_CM * X_MH; - - Matx33f R = X_CH.R * X_CH_0.R.t(); - Vec3f t = X_CH.t - X_CH_0.t; - - // correct for camera pitch - Matx33f R_CP( 1, 0, 0, - 0, cos(deg2rad*cam_pitch), sin(deg2rad*cam_pitch), - 0, -sin(deg2rad*cam_pitch), cos(deg2rad*cam_pitch)); - R = R_CP * R * R_CP.t(); - t = R_CP * t; - - // get translation(s) - if (bEnableX) { - data[TX] = t[0] / 10.0; // convert to cm - } - if (bEnableY) { - data[TY] = t[1] / 10.0; - } - if (bEnableZ) { - data[TZ] = t[2] / 10.0; - } - - // translate rotation matrix from opengl (G) to roll-pitch-yaw (R) frame - // -z -> x, y -> z, x -> -y - Matx33f R_RG( 0, 0,-1, - -1, 0, 0, - 0, 1, 0); - R = R_RG * R * R_RG.t(); - - // extract rotation angles - double alpha, beta, gamma; - //beta = atan2( -R(2,0), sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) ); - 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)); - - if (bEnableYaw) { - data[Yaw] = rad2deg * alpha; - } - if (bEnablePitch) { - data[Pitch] = rad2deg * beta; - } - if (bEnableRoll) { - data[Roll] = rad2deg * gamma; - } - } - return true; -} - -//----------------------------------------------------------------------------- -//#pragma comment(linker, "/export:GetTracker=_GetTracker@0") - -extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor() -{ - return new Tracker; -} +/* 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. + */ + +#include "ftnoir_tracker_pt.h" +#include +#include +#include +#include +#include +#include "facetracknoir/global-settings.h" + +using namespace std; +using namespace cv; + +//#define PT_PERF_LOG //log performance + +//----------------------------------------------------------------------------- +Tracker::Tracker() + : tracking_valid(false), frame_count(0), commands(0), video_widget(NULL), fresh(false) +{ + should_quit = false; + qDebug()<<"Tracker::Tracker"; + TrackerSettings settings; + settings.load_ini(); + apply(settings); +} + +Tracker::~Tracker() +{ + qDebug()<<"Tracker::~Tracker"; + set_command(ABORT); + wait(); + if (video_widget) delete video_widget; +} + +void Tracker::set_command(Command command) +{ + //QMutexLocker lock(&mutex); + commands |= command; +} + +void Tracker::reset_command(Command command) +{ + //QMutexLocker lock(&mutex); + commands &= ~command; +} + +void Tracker::run() +{ + qDebug()<<"Tracker:: Thread started"; + +#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 + + time.start(); + float dt; + bool new_frame; + forever + { + { + QMutexLocker lock(&mutex); + if (should_quit) + break; + + if (commands & ABORT) break; + if (commands & PAUSE) continue; + commands = 0; + + dt = time.elapsed() / 1000.0; + time.restart(); + + new_frame = camera.get_frame(dt, &frame); + if (new_frame && !frame.empty()) + { + const std::vector& points = point_extractor.extract_points(frame, dt, draw_frame); + tracking_valid = point_tracker.track(points, camera.get_info().f, dt); + frame_count++; + fresh = true; + } +#ifdef PT_PERF_LOG + log_stream<<"dt: "<(new PointModel(settings.M01, settings.M02)); + point_tracker.dynamic_pose_resolution = settings.dyn_pose_res; + sleep_time = settings.sleep_time; + point_tracker.dt_reset = settings.reset_time / 1000.0; + draw_frame = settings.video_widget; + cam_pitch = settings.cam_pitch; + + bEnableRoll = settings.bEnableRoll; + bEnablePitch = settings.bEnablePitch; + bEnableYaw = settings.bEnableYaw; + bEnableX = settings.bEnableX; + bEnableY = settings.bEnableY; + bEnableZ = settings.bEnableZ; + + t_MH = settings.t_MH; + qDebug()<<"Tracker::apply ends"; +} + +void Tracker::reset() +{ + QMutexLocker lock(&mutex); + point_tracker.reset(); +} + +void Tracker::center() +{ + point_tracker.reset(); // we also do a reset here since there is no reset shortkey yet + QMutexLocker lock(&mutex); + FrameTrafo X_CM_0 = point_tracker.get_pose(); + FrameTrafo X_MH(Matx33f::eye(), t_MH); + X_CH_0 = X_CM_0 * X_MH; +} + +void Tracker::refreshVideo() +{ + if (video_widget && fresh) + { + Mat frame_copy; + std::auto_ptr< vector > points; + { + //QMutexLocker lock(&mutex); + if (!draw_frame || frame.empty()) return; + + // copy the frame and points from the tracker thread + frame_copy = frame.clone(); + points = std::auto_ptr< vector >(new vector(point_extractor.get_points())); + } + //QMutexLocker lck(&mutex); + video_widget->update_image(frame_copy, points); + } +} + +void Tracker::StartTracker(QFrame* videoframe) +{ + const int VIDEO_FRAME_WIDTH = videoframe->width(); + const int VIDEO_FRAME_HEIGHT = videoframe->height(); + TrackerSettings settings; + settings.load_ini(); + apply(settings); + qDebug("Tracker::Initialize"); + // setup video frame + video_widget = new VideoWidget(videoframe); + QHBoxLayout* layout = new QHBoxLayout(); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(video_widget); + if (videoframe->layout()) delete videoframe->layout(); + videoframe->setLayout(layout); + video_widget->resize(VIDEO_FRAME_WIDTH, VIDEO_FRAME_HEIGHT); + videoframe->show(); + connect(&timer, SIGNAL(timeout()), this, SLOT(paint_widget())); + timer.start(40); + camera.start(); + start(); + reset_command(PAUSE); +} + +void Tracker::paint_widget() { + if (fresh) { + fresh = false; + video_widget->update(); + } +} + +bool Tracker::GiveHeadPoseData(double *data) +{ + const double rad2deg = 180.0/3.14159265; + const double deg2rad = 1.0/rad2deg; + { + QMutexLocker lock(&mutex); + + if (!tracking_valid) return false; + + FrameTrafo X_CM = point_tracker.get_pose(); + FrameTrafo X_MH(Matx33f::eye(), t_MH); + FrameTrafo X_CH = X_CM * X_MH; + + Matx33f R = X_CH.R * X_CH_0.R.t(); + Vec3f t = X_CH.t - X_CH_0.t; + + // correct for camera pitch + Matx33f R_CP( 1, 0, 0, + 0, cos(deg2rad*cam_pitch), sin(deg2rad*cam_pitch), + 0, -sin(deg2rad*cam_pitch), cos(deg2rad*cam_pitch)); + R = R_CP * R * R_CP.t(); + t = R_CP * t; + + // get translation(s) + if (bEnableX) { + data[TX] = t[0] / 10.0; // convert to cm + } + if (bEnableY) { + data[TY] = t[1] / 10.0; + } + if (bEnableZ) { + data[TZ] = t[2] / 10.0; + } + + // translate rotation matrix from opengl (G) to roll-pitch-yaw (R) frame + // -z -> x, y -> z, x -> -y + Matx33f R_RG( 0, 0,-1, + -1, 0, 0, + 0, 1, 0); + R = R_RG * R * R_RG.t(); + + // extract rotation angles + double alpha, beta, gamma; + //beta = atan2( -R(2,0), sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) ); + 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)); + + if (bEnableYaw) { + data[Yaw] = rad2deg * alpha; + } + if (bEnablePitch) { + data[Pitch] = rad2deg * beta; + } + if (bEnableRoll) { + data[Roll] = rad2deg * gamma; + } + } + return true; +} + +//----------------------------------------------------------------------------- +//#pragma comment(linker, "/export:GetTracker=_GetTracker@0") + +extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor() +{ + return new Tracker; +} diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h index 0c45c6b6..3825fce7 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -1,96 +1,96 @@ -/* 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. - */ - -#ifndef FTNOIR_TRACKER_PT_H -#define FTNOIR_TRACKER_PT_H - -#include "ftnoir_tracker_base/ftnoir_tracker_base.h" -#include "ftnoir_tracker_pt_settings.h" -#include "camera.h" -#include "point_extractor.h" -#include "point_tracker.h" -#include "video_widget.h" -#include "timer.h" - -#include -#include -#include -#include -#include -#include - -//----------------------------------------------------------------------------- -class Tracker : public QThread, public ITracker -{ - Q_OBJECT -public: - Tracker(); - ~Tracker(); - - // ITracker interface - void StartTracker(QFrame* videoFrame); - bool GiveHeadPoseData(double *data); - - void refreshVideo(); - - void apply(const TrackerSettings& settings); - void center(); - void reset(); // reset the trackers internal state variables - void run(); - void WaitForExit() { - should_quit = true; - wait(); - } - - void get_pose(FrameTrafo* X_CM) { QMutexLocker lock(&mutex); *X_CM = point_tracker.get_pose(); } - int get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); } - void get_cam_info(CamInfo* info) { QMutexLocker lock(&mutex); *info = camera.get_info(); } - -protected: - FrameTrafo X_CH_0; // for centering - cv::Mat frame; // the output frame for display - - enum Command { - ABORT = 1<<0, - PAUSE = 1<<1 - }; - void set_command(Command command); - void reset_command(Command command); - - CVCamera camera; - PointExtractor point_extractor; - PointTracker point_tracker; - bool tracking_valid; - - cv::Vec3f t_MH; - int cam_pitch; - - bool draw_frame; - int sleep_time; - - bool bEnableRoll; - bool bEnablePitch; - bool bEnableYaw; - bool bEnableX; - bool bEnableY; - bool bEnableZ; - - long frame_count; - volatile int commands; - - VideoWidget* video_widget; - Timer time; - QMutex mutex; - volatile bool should_quit; - volatile bool fresh; - QTimer timer; - -protected slots: - void paint_widget(); -}; - -#endif // FTNOIR_TRACKER_PT_H +/* 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. + */ + +#ifndef FTNOIR_TRACKER_PT_H +#define FTNOIR_TRACKER_PT_H + +#include "ftnoir_tracker_base/ftnoir_tracker_base.h" +#include "ftnoir_tracker_pt_settings.h" +#include "camera.h" +#include "point_extractor.h" +#include "point_tracker.h" +#include "video_widget.h" +#include "timer.h" + +#include +#include +#include +#include +#include +#include + +//----------------------------------------------------------------------------- +class Tracker : public QThread, public ITracker +{ + Q_OBJECT +public: + Tracker(); + ~Tracker(); + + // ITracker interface + void StartTracker(QFrame* videoFrame); + bool GiveHeadPoseData(double *data); + + void refreshVideo(); + + void apply(const TrackerSettings& settings); + void center(); + void reset(); // reset the trackers internal state variables + void run(); + void WaitForExit() { + should_quit = true; + wait(); + } + + void get_pose(FrameTrafo* X_CM) { QMutexLocker lock(&mutex); *X_CM = point_tracker.get_pose(); } + int get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); } + void get_cam_info(CamInfo* info) { QMutexLocker lock(&mutex); *info = camera.get_info(); } + +protected: + FrameTrafo X_CH_0; // for centering + cv::Mat frame; // the output frame for display + + enum Command { + ABORT = 1<<0, + PAUSE = 1<<1 + }; + void set_command(Command command); + void reset_command(Command command); + + CVCamera camera; + PointExtractor point_extractor; + PointTracker point_tracker; + bool tracking_valid; + + cv::Vec3f t_MH; + int cam_pitch; + + bool draw_frame; + int sleep_time; + + bool bEnableRoll; + bool bEnablePitch; + bool bEnableYaw; + bool bEnableX; + bool bEnableY; + bool bEnableZ; + + long frame_count; + volatile int commands; + + VideoWidget* video_widget; + Timer time; + QMutex mutex; + volatile bool should_quit; + volatile bool fresh; + QTimer timer; + +protected slots: + void paint_widget(); +}; + +#endif // FTNOIR_TRACKER_PT_H diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp index 4837f4a9..f222085e 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -1,337 +1,337 @@ -/* 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. - */ - -#include "ftnoir_tracker_pt_dialog.h" - -#include -#include -#include "facetracknoir/global-settings.h" - -//----------------------------------------------------------------------------- -TrackerDialog::TrackerDialog() - : settings_dirty(false), tracker(NULL), trans_calib_running(false), timer(this) -{ - qDebug()<<"TrackerDialog::TrackerDialog"; - setAttribute(Qt::WA_DeleteOnClose, false); - - ui.setupUi( this ); - - settings.load_ini(); - dialog_settings.load_ini(); - - // initialize ui values - ui.videowidget_check->setChecked(settings.video_widget); - ui.dynpose_check->setChecked(settings.dyn_pose_res); - ui.sleep_spin->setValue(settings.sleep_time); - ui.reset_spin->setValue(settings.reset_time); - ui.camindex_spin->setValue(settings.cam_index); - ui.f_dspin->setValue(settings.cam_f); - ui.res_x_spin->setValue(settings.cam_res_x); - ui.res_y_spin->setValue(settings.cam_res_y); - ui.fps_spin->setValue(settings.cam_fps); - ui.campitch_spin->setValue(settings.cam_pitch); - ui.threshold_slider->setValue(settings.threshold); - - ui.chkEnableRoll->setChecked(settings.bEnableRoll); - ui.chkEnablePitch->setChecked(settings.bEnablePitch); - ui.chkEnableYaw->setChecked(settings.bEnableYaw); - ui.chkEnableX->setChecked(settings.bEnableX); - ui.chkEnableY->setChecked(settings.bEnableY); - ui.chkEnableZ->setChecked(settings.bEnableZ); - - ui.mindiam_spin->setValue(settings.min_point_size); - ui.maxdiam_spin->setValue(settings.max_point_size); - ui.model_tabs->setCurrentIndex(dialog_settings.active_model_panel); - ui.clip_bheight_spin->setValue(dialog_settings.clip_by); - ui.clip_blength_spin->setValue(dialog_settings.clip_bz); - ui.clip_theight_spin->setValue(dialog_settings.clip_ty); - ui.clip_tlength_spin->setValue(dialog_settings.clip_tz); - ui.cap_width_spin->setValue(dialog_settings.cap_x); - ui.cap_height_spin->setValue(dialog_settings.cap_y); - ui.cap_length_spin->setValue(dialog_settings.cap_z); - ui.m1x_spin->setValue(dialog_settings.M01x); - ui.m1y_spin->setValue(dialog_settings.M01y); - ui.m1z_spin->setValue(dialog_settings.M01z); - ui.m2x_spin->setValue(dialog_settings.M02x); - ui.m2y_spin->setValue(dialog_settings.M02y); - ui.m2z_spin->setValue(dialog_settings.M02z); - ui.tx_spin->setValue(settings.t_MH[0]); - ui.ty_spin->setValue(settings.t_MH[1]); - ui.tz_spin->setValue(settings.t_MH[2]); - - // connect Qt signals and slots - connect( ui.videowidget_check,SIGNAL(toggled(bool)), this,SLOT(set_video_widget(bool)) ); - connect( ui.dynpose_check,SIGNAL(toggled(bool)), this,SLOT(set_dyn_pose_res(bool)) ); - connect( ui.sleep_spin,SIGNAL(valueChanged(int)), this,SLOT(set_sleep_time(int)) ); - connect( ui.reset_spin,SIGNAL(valueChanged(int)), this,SLOT(set_reset_time(int)) ); - connect( ui.camindex_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_index(int)) ); - connect( ui.f_dspin,SIGNAL(valueChanged(double)), this,SLOT(set_cam_f(double)) ); - connect( ui.res_x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_x(int)) ); - connect( ui.res_y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_y(int)) ); - connect( ui.fps_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_fps(int)) ); - connect( ui.campitch_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_pitch(int)) ); - connect( ui.threshold_slider,SIGNAL(sliderMoved(int)), this,SLOT(set_threshold(int)) ); - - connect( ui.chkEnableRoll,SIGNAL(toggled(bool)), this,SLOT(set_ena_roll(bool)) ); - connect( ui.chkEnablePitch,SIGNAL(toggled(bool)), this,SLOT(set_ena_pitch(bool)) ); - connect( ui.chkEnableYaw,SIGNAL(toggled(bool)), this,SLOT(set_ena_yaw(bool)) ); - connect( ui.chkEnableX,SIGNAL(toggled(bool)), this,SLOT(set_ena_x(bool)) ); - connect( ui.chkEnableY,SIGNAL(toggled(bool)), this,SLOT(set_ena_y(bool)) ); - connect( ui.chkEnableZ,SIGNAL(toggled(bool)), this,SLOT(set_ena_z(bool)) ); - - connect( ui.mindiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_min_point_size(int)) ); - connect( ui.maxdiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_max_point_size(int)) ); - connect( ui.model_tabs,SIGNAL(currentChanged(int)), this,SLOT(set_model(int)) ); - connect( ui.clip_theight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_height(int)) ); - connect( ui.clip_tlength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_length(int)) ); - connect( ui.clip_bheight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_height(int)) ); - connect( ui.clip_blength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_length(int)) ); - connect( ui.cap_width_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_width(int)) ); - connect( ui.cap_height_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_height(int)) ); - connect( ui.cap_length_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_length(int)) ); - connect( ui.m1x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1x(int)) ); - connect( ui.m1y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1y(int)) ); - connect( ui.m1z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1z(int)) ); - connect( ui.m2x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2x(int)) ); - connect( ui.m2y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2y(int)) ); - connect( ui.m2z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2z(int)) ); - connect( ui.tx_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tx(int)) ); - connect( ui.ty_spin,SIGNAL(valueChanged(int)), this,SLOT(set_ty(int)) ); - connect( ui.tz_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tz(int)) ); - - connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) ); - - connect(ui.reset_button, SIGNAL(clicked()), this, SLOT(doReset())); - //connect(ui.center_button, SIGNAL(clicked()), this, SLOT(doCenter())); - - connect(ui.ok_button, SIGNAL(clicked()), this, SLOT(doOK())); - connect(ui.cancel_button, SIGNAL(clicked()), this, SLOT(doCancel())); - - connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info())); - timer.start(100); -} - -TrackerDialog::~TrackerDialog() -{ - qDebug()<<"TrackerDialog::~TrackerDialog"; -} - -void TrackerDialog::set_clip() -{ - settings.M01[0] = 0; - settings.M01[1] = dialog_settings.clip_ty; - settings.M01[2] = -dialog_settings.clip_tz; - settings.M02[0] = 0; - settings.M02[1] = -dialog_settings.clip_by; - settings.M02[2] = -dialog_settings.clip_bz; - - settings_changed(); -} - -void TrackerDialog::set_cap() -{ - settings.M01[0] = -dialog_settings.cap_x; - settings.M01[1] = -dialog_settings.cap_y; - settings.M01[2] = -dialog_settings.cap_z; - settings.M02[0] = dialog_settings.cap_x; - settings.M02[1] = -dialog_settings.cap_y; - settings.M02[2] = -dialog_settings.cap_z; - - settings_changed(); -} - -void TrackerDialog::set_custom() -{ - settings.M01[0] = dialog_settings.M01x; - settings.M01[1] = dialog_settings.M01y; - settings.M01[2] = dialog_settings.M01z; - settings.M02[0] = dialog_settings.M02x; - settings.M02[1] = dialog_settings.M02y; - settings.M02[2] = dialog_settings.M02z; - - settings_changed(); -} - -void TrackerDialog::set_model(int val) -{ - dialog_settings.active_model_panel = val; - - switch (val) { - - case TrackerDialogSettings::MODEL_CLIP: - set_clip(); - break; - - case TrackerDialogSettings::MODEL_CAP: - set_cap(); - break; - - case TrackerDialogSettings::MODEL_CUSTOM: - set_custom(); - break; - - default: - break; - } -} - -void TrackerDialog::startstop_trans_calib(bool start) -{ - if (start) - { - qDebug()<<"TrackerDialog:: Starting translation calibration"; - trans_calib.reset(); - trans_calib_running = true; - } - else - { - qDebug()<<"TrackerDialog:: Stoppping translation calibration"; - trans_calib_running = false; - settings.t_MH = trans_calib.get_estimate(); - settings_changed(); - } -} - -void TrackerDialog::trans_calib_step() -{ - if (tracker) - { - FrameTrafo X_CM; - tracker->get_pose(&X_CM); - trans_calib.update(X_CM.R, X_CM.t); - cv::Vec3f t_MH = trans_calib.get_estimate(); - //qDebug()<<"TrackerDialog:: Current translation estimate: "<setValue(t_MH[0]); - ui.ty_spin->setValue(t_MH[1]); - ui.tz_spin->setValue(t_MH[2]); - } -} - -void TrackerDialog::settings_changed() -{ - settings_dirty = true; - if (tracker) tracker->apply(settings); -} - -void TrackerDialog::doCenter() -{ - if (tracker) tracker->center(); -} - -void TrackerDialog::doReset() -{ - if (tracker) tracker->reset(); -} - -void TrackerDialog::doOK() -{ - settings.save_ini(); - dialog_settings.save_ini(); - close(); -} - -void TrackerDialog::doCancel() -{ - if (settings_dirty) { - int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", - QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard ); - switch (ret) { - case QMessageBox::Save: - settings.save_ini(); - dialog_settings.save_ini(); - close(); - break; - case QMessageBox::Discard: - close(); - break; - case QMessageBox::Cancel: - // Cancel was clicked - break; - default: - // should never be reached - break; - } - } - else { - close(); - } -} - -void TrackerDialog::poll_tracker_info() -{ - if (tracker) - { - QString to_print; - - // display caminfo - CamInfo info; - tracker->get_cam_info(&info); - to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS"; - ui.caminfo_label->setText(to_print); - ui.caminfo_label_2->setText(to_print); - - // display pointinfo - int n_points = tracker->get_n_points(); - to_print = QString::number(n_points); - if (n_points == 3) - to_print += " OK!"; - else - to_print += " BAD!"; - ui.pointinfo_label->setText(to_print); - ui.pointinfo_label_2->setText(to_print); - - // update calibration - if (trans_calib_running) trans_calib_step(); - } - else - { - QString to_print = "Tracker offline"; - ui.caminfo_label->setText(to_print); - ui.caminfo_label_2->setText(to_print); - ui.pointinfo_label->setText(to_print); - ui.pointinfo_label_2->setText(to_print); - } -} - - -//----------------------------------------------------------------------------- -// ITrackerDialog interface -void TrackerDialog::Initialize(QWidget *parent) -{ - QPoint offsetpos(200, 200); - if (parent) { - this->move(parent->pos() + offsetpos); - } - show(); -} - -void TrackerDialog::registerTracker(ITracker *t) -{ - qDebug()<<"TrackerDialog:: Tracker registered"; - tracker = static_cast(t); - if (isVisible() && settings_dirty) tracker->apply(settings); - ui.tcalib_button->setEnabled(true); - //ui.center_button->setEnabled(true); - ui.reset_button->setEnabled(true); -} - -void TrackerDialog::unRegisterTracker() -{ - qDebug()<<"TrackerDialog:: Tracker un-registered"; - tracker = NULL; - ui.tcalib_button->setEnabled(false); - //ui.center_button->setEnabled(false); - ui.reset_button->setEnabled(false); -} - -//----------------------------------------------------------------------------- -//#pragma comment(linker, "/export:GetTrackerDialog=_GetTrackerDialog@0") - -extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDialog( ) -{ - return new TrackerDialog; -} +/* 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. + */ + +#include "ftnoir_tracker_pt_dialog.h" + +#include +#include +#include "facetracknoir/global-settings.h" + +//----------------------------------------------------------------------------- +TrackerDialog::TrackerDialog() + : settings_dirty(false), tracker(NULL), trans_calib_running(false), timer(this) +{ + qDebug()<<"TrackerDialog::TrackerDialog"; + setAttribute(Qt::WA_DeleteOnClose, false); + + ui.setupUi( this ); + + settings.load_ini(); + dialog_settings.load_ini(); + + // initialize ui values + ui.videowidget_check->setChecked(settings.video_widget); + ui.dynpose_check->setChecked(settings.dyn_pose_res); + ui.sleep_spin->setValue(settings.sleep_time); + ui.reset_spin->setValue(settings.reset_time); + ui.camindex_spin->setValue(settings.cam_index); + ui.f_dspin->setValue(settings.cam_f); + ui.res_x_spin->setValue(settings.cam_res_x); + ui.res_y_spin->setValue(settings.cam_res_y); + ui.fps_spin->setValue(settings.cam_fps); + ui.campitch_spin->setValue(settings.cam_pitch); + ui.threshold_slider->setValue(settings.threshold); + + ui.chkEnableRoll->setChecked(settings.bEnableRoll); + ui.chkEnablePitch->setChecked(settings.bEnablePitch); + ui.chkEnableYaw->setChecked(settings.bEnableYaw); + ui.chkEnableX->setChecked(settings.bEnableX); + ui.chkEnableY->setChecked(settings.bEnableY); + ui.chkEnableZ->setChecked(settings.bEnableZ); + + ui.mindiam_spin->setValue(settings.min_point_size); + ui.maxdiam_spin->setValue(settings.max_point_size); + ui.model_tabs->setCurrentIndex(dialog_settings.active_model_panel); + ui.clip_bheight_spin->setValue(dialog_settings.clip_by); + ui.clip_blength_spin->setValue(dialog_settings.clip_bz); + ui.clip_theight_spin->setValue(dialog_settings.clip_ty); + ui.clip_tlength_spin->setValue(dialog_settings.clip_tz); + ui.cap_width_spin->setValue(dialog_settings.cap_x); + ui.cap_height_spin->setValue(dialog_settings.cap_y); + ui.cap_length_spin->setValue(dialog_settings.cap_z); + ui.m1x_spin->setValue(dialog_settings.M01x); + ui.m1y_spin->setValue(dialog_settings.M01y); + ui.m1z_spin->setValue(dialog_settings.M01z); + ui.m2x_spin->setValue(dialog_settings.M02x); + ui.m2y_spin->setValue(dialog_settings.M02y); + ui.m2z_spin->setValue(dialog_settings.M02z); + ui.tx_spin->setValue(settings.t_MH[0]); + ui.ty_spin->setValue(settings.t_MH[1]); + ui.tz_spin->setValue(settings.t_MH[2]); + + // connect Qt signals and slots + connect( ui.videowidget_check,SIGNAL(toggled(bool)), this,SLOT(set_video_widget(bool)) ); + connect( ui.dynpose_check,SIGNAL(toggled(bool)), this,SLOT(set_dyn_pose_res(bool)) ); + connect( ui.sleep_spin,SIGNAL(valueChanged(int)), this,SLOT(set_sleep_time(int)) ); + connect( ui.reset_spin,SIGNAL(valueChanged(int)), this,SLOT(set_reset_time(int)) ); + connect( ui.camindex_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_index(int)) ); + connect( ui.f_dspin,SIGNAL(valueChanged(double)), this,SLOT(set_cam_f(double)) ); + connect( ui.res_x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_x(int)) ); + connect( ui.res_y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_y(int)) ); + connect( ui.fps_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_fps(int)) ); + connect( ui.campitch_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_pitch(int)) ); + connect( ui.threshold_slider,SIGNAL(sliderMoved(int)), this,SLOT(set_threshold(int)) ); + + connect( ui.chkEnableRoll,SIGNAL(toggled(bool)), this,SLOT(set_ena_roll(bool)) ); + connect( ui.chkEnablePitch,SIGNAL(toggled(bool)), this,SLOT(set_ena_pitch(bool)) ); + connect( ui.chkEnableYaw,SIGNAL(toggled(bool)), this,SLOT(set_ena_yaw(bool)) ); + connect( ui.chkEnableX,SIGNAL(toggled(bool)), this,SLOT(set_ena_x(bool)) ); + connect( ui.chkEnableY,SIGNAL(toggled(bool)), this,SLOT(set_ena_y(bool)) ); + connect( ui.chkEnableZ,SIGNAL(toggled(bool)), this,SLOT(set_ena_z(bool)) ); + + connect( ui.mindiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_min_point_size(int)) ); + connect( ui.maxdiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_max_point_size(int)) ); + connect( ui.model_tabs,SIGNAL(currentChanged(int)), this,SLOT(set_model(int)) ); + connect( ui.clip_theight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_height(int)) ); + connect( ui.clip_tlength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_length(int)) ); + connect( ui.clip_bheight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_height(int)) ); + connect( ui.clip_blength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_length(int)) ); + connect( ui.cap_width_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_width(int)) ); + connect( ui.cap_height_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_height(int)) ); + connect( ui.cap_length_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_length(int)) ); + connect( ui.m1x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1x(int)) ); + connect( ui.m1y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1y(int)) ); + connect( ui.m1z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1z(int)) ); + connect( ui.m2x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2x(int)) ); + connect( ui.m2y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2y(int)) ); + connect( ui.m2z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2z(int)) ); + connect( ui.tx_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tx(int)) ); + connect( ui.ty_spin,SIGNAL(valueChanged(int)), this,SLOT(set_ty(int)) ); + connect( ui.tz_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tz(int)) ); + + connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) ); + + connect(ui.reset_button, SIGNAL(clicked()), this, SLOT(doReset())); + //connect(ui.center_button, SIGNAL(clicked()), this, SLOT(doCenter())); + + connect(ui.ok_button, SIGNAL(clicked()), this, SLOT(doOK())); + connect(ui.cancel_button, SIGNAL(clicked()), this, SLOT(doCancel())); + + connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info())); + timer.start(100); +} + +TrackerDialog::~TrackerDialog() +{ + qDebug()<<"TrackerDialog::~TrackerDialog"; +} + +void TrackerDialog::set_clip() +{ + settings.M01[0] = 0; + settings.M01[1] = dialog_settings.clip_ty; + settings.M01[2] = -dialog_settings.clip_tz; + settings.M02[0] = 0; + settings.M02[1] = -dialog_settings.clip_by; + settings.M02[2] = -dialog_settings.clip_bz; + + settings_changed(); +} + +void TrackerDialog::set_cap() +{ + settings.M01[0] = -dialog_settings.cap_x; + settings.M01[1] = -dialog_settings.cap_y; + settings.M01[2] = -dialog_settings.cap_z; + settings.M02[0] = dialog_settings.cap_x; + settings.M02[1] = -dialog_settings.cap_y; + settings.M02[2] = -dialog_settings.cap_z; + + settings_changed(); +} + +void TrackerDialog::set_custom() +{ + settings.M01[0] = dialog_settings.M01x; + settings.M01[1] = dialog_settings.M01y; + settings.M01[2] = dialog_settings.M01z; + settings.M02[0] = dialog_settings.M02x; + settings.M02[1] = dialog_settings.M02y; + settings.M02[2] = dialog_settings.M02z; + + settings_changed(); +} + +void TrackerDialog::set_model(int val) +{ + dialog_settings.active_model_panel = val; + + switch (val) { + + case TrackerDialogSettings::MODEL_CLIP: + set_clip(); + break; + + case TrackerDialogSettings::MODEL_CAP: + set_cap(); + break; + + case TrackerDialogSettings::MODEL_CUSTOM: + set_custom(); + break; + + default: + break; + } +} + +void TrackerDialog::startstop_trans_calib(bool start) +{ + if (start) + { + qDebug()<<"TrackerDialog:: Starting translation calibration"; + trans_calib.reset(); + trans_calib_running = true; + } + else + { + qDebug()<<"TrackerDialog:: Stoppping translation calibration"; + trans_calib_running = false; + settings.t_MH = trans_calib.get_estimate(); + settings_changed(); + } +} + +void TrackerDialog::trans_calib_step() +{ + if (tracker) + { + FrameTrafo X_CM; + tracker->get_pose(&X_CM); + trans_calib.update(X_CM.R, X_CM.t); + cv::Vec3f t_MH = trans_calib.get_estimate(); + //qDebug()<<"TrackerDialog:: Current translation estimate: "<setValue(t_MH[0]); + ui.ty_spin->setValue(t_MH[1]); + ui.tz_spin->setValue(t_MH[2]); + } +} + +void TrackerDialog::settings_changed() +{ + settings_dirty = true; + if (tracker) tracker->apply(settings); +} + +void TrackerDialog::doCenter() +{ + if (tracker) tracker->center(); +} + +void TrackerDialog::doReset() +{ + if (tracker) tracker->reset(); +} + +void TrackerDialog::doOK() +{ + settings.save_ini(); + dialog_settings.save_ini(); + close(); +} + +void TrackerDialog::doCancel() +{ + if (settings_dirty) { + int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", + QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard ); + switch (ret) { + case QMessageBox::Save: + settings.save_ini(); + dialog_settings.save_ini(); + close(); + break; + case QMessageBox::Discard: + close(); + break; + case QMessageBox::Cancel: + // Cancel was clicked + break; + default: + // should never be reached + break; + } + } + else { + close(); + } +} + +void TrackerDialog::poll_tracker_info() +{ + if (tracker) + { + QString to_print; + + // display caminfo + CamInfo info; + tracker->get_cam_info(&info); + to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS"; + ui.caminfo_label->setText(to_print); + ui.caminfo_label_2->setText(to_print); + + // display pointinfo + int n_points = tracker->get_n_points(); + to_print = QString::number(n_points); + if (n_points == 3) + to_print += " OK!"; + else + to_print += " BAD!"; + ui.pointinfo_label->setText(to_print); + ui.pointinfo_label_2->setText(to_print); + + // update calibration + if (trans_calib_running) trans_calib_step(); + } + else + { + QString to_print = "Tracker offline"; + ui.caminfo_label->setText(to_print); + ui.caminfo_label_2->setText(to_print); + ui.pointinfo_label->setText(to_print); + ui.pointinfo_label_2->setText(to_print); + } +} + + +//----------------------------------------------------------------------------- +// ITrackerDialog interface +void TrackerDialog::Initialize(QWidget *parent) +{ + QPoint offsetpos(200, 200); + if (parent) { + this->move(parent->pos() + offsetpos); + } + show(); +} + +void TrackerDialog::registerTracker(ITracker *t) +{ + qDebug()<<"TrackerDialog:: Tracker registered"; + tracker = static_cast(t); + if (isVisible() && settings_dirty) tracker->apply(settings); + ui.tcalib_button->setEnabled(true); + //ui.center_button->setEnabled(true); + ui.reset_button->setEnabled(true); +} + +void TrackerDialog::unRegisterTracker() +{ + qDebug()<<"TrackerDialog:: Tracker un-registered"; + tracker = NULL; + ui.tcalib_button->setEnabled(false); + //ui.center_button->setEnabled(false); + ui.reset_button->setEnabled(false); +} + +//----------------------------------------------------------------------------- +//#pragma comment(linker, "/export:GetTrackerDialog=_GetTrackerDialog@0") + +extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDialog( ) +{ + return new TrackerDialog; +} diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h index ada331e5..f13fe13e 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h @@ -1,102 +1,102 @@ -/* 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. - */ - -#ifndef FTNOIR_TRACKER_PT_DIALOG_H -#define FTNOIR_TRACKER_PT_DIALOG_H - -#include "ftnoir_tracker_base/ftnoir_tracker_base.h" -#include "ftnoir_tracker_pt_settings.h" -#include "ftnoir_tracker_pt.h" -#include "ui_ftnoir_pt_controls.h" -#include "trans_calib.h" - -#include - -//----------------------------------------------------------------------------- -class TrackerDialog : public QWidget, public ITrackerDialog -{ - Q_OBJECT -public: - TrackerDialog(); - ~TrackerDialog(); - - // ITrackerDialog interface - void Initialize(QWidget *parent); - void registerTracker(ITracker *tracker); - void unRegisterTracker(); - - void trans_calib_step(); - -protected slots: - // ugly qt stuff - void set_video_widget(bool val) { settings.video_widget = val; settings_changed(); } - void set_dyn_pose_res(bool val) { settings.dyn_pose_res = val; settings_changed(); } - void set_sleep_time(int val) { settings.sleep_time = val; settings_changed(); } - void set_reset_time(int val) { settings.reset_time = val; settings_changed(); } - void set_cam_index(int val) { settings.cam_index = val; settings_changed(); } - void set_cam_f(double val) { settings.cam_f = val; settings_changed(); } - void set_cam_res_x(int val) { settings.cam_res_x = val; settings_changed(); } - void set_cam_res_y(int val) { settings.cam_res_y = val; settings_changed(); } - void set_cam_fps(int val) { settings.cam_fps = val; settings_changed(); } - void set_cam_pitch(int val) { settings.cam_pitch = val; settings_changed(); } - void set_min_point_size(int val) { settings.min_point_size = val; settings_changed(); } - void set_max_point_size(int val) { settings.max_point_size = val; settings_changed(); } - void set_threshold(int val) { settings.threshold = val; settings_changed(); } - void set_ena_roll(bool val) { settings.bEnableRoll = val; settings_changed(); } - void set_ena_pitch(bool val) { settings.bEnablePitch = val; settings_changed(); } - void set_ena_yaw(bool val) { settings.bEnableYaw = val; settings_changed(); } - void set_ena_x(bool val) { settings.bEnableX = val; settings_changed(); } - void set_ena_y(bool val) { settings.bEnableY = val; settings_changed(); } - void set_ena_z(bool val) { settings.bEnableZ = val; settings_changed(); } - - void set_clip_t_height(int val) { dialog_settings.clip_ty = val; set_clip(); } - void set_clip_t_length(int val) { dialog_settings.clip_tz = val; set_clip(); } - void set_clip_b_height(int val) { dialog_settings.clip_by = val; set_clip(); } - void set_clip_b_length(int val) { dialog_settings.clip_bz = val; set_clip(); } - void set_cap_width(int val) { dialog_settings.cap_x = val; set_cap(); } - void set_cap_height(int val) { dialog_settings.cap_y = val; set_cap(); } - void set_cap_length(int val) { dialog_settings.cap_z = val; set_cap(); } - void set_m1x(int val) { dialog_settings.M01x = val; set_custom(); } - void set_m1y(int val) { dialog_settings.M01y = val; set_custom(); } - void set_m1z(int val) { dialog_settings.M01z = val; set_custom(); } - void set_m2x(int val) { dialog_settings.M02x = val; set_custom(); } - void set_m2y(int val) { dialog_settings.M02y = val; set_custom(); } - void set_m2z(int val) { dialog_settings.M02z = val; set_custom(); } - void set_tx(int val) { settings.t_MH[0] = val; settings_changed(); } - void set_ty(int val) { settings.t_MH[1] = val; settings_changed(); } - void set_tz(int val) { settings.t_MH[2] = val; settings_changed(); } - void set_model(int model_id); - - void doCenter(); - void doReset(); - - void doOK(); - void doCancel(); - - void startstop_trans_calib(bool start); - - void poll_tracker_info(); - -protected: - void set_clip(); - void set_cap(); - void set_custom(); - - void settings_changed(); - - TrackerSettings settings; - TrackerDialogSettings dialog_settings; - bool settings_dirty; - - Tracker* tracker; - TranslationCalibrator trans_calib; - bool trans_calib_running; - QTimer timer; - Ui::UICPTClientControls ui; -}; - -#endif //FTNOIR_TRACKER_PT_DIALOG_H +/* 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. + */ + +#ifndef FTNOIR_TRACKER_PT_DIALOG_H +#define FTNOIR_TRACKER_PT_DIALOG_H + +#include "ftnoir_tracker_base/ftnoir_tracker_base.h" +#include "ftnoir_tracker_pt_settings.h" +#include "ftnoir_tracker_pt.h" +#include "ui_ftnoir_pt_controls.h" +#include "trans_calib.h" + +#include + +//----------------------------------------------------------------------------- +class TrackerDialog : public QWidget, public ITrackerDialog +{ + Q_OBJECT +public: + TrackerDialog(); + ~TrackerDialog(); + + // ITrackerDialog interface + void Initialize(QWidget *parent); + void registerTracker(ITracker *tracker); + void unRegisterTracker(); + + void trans_calib_step(); + +protected slots: + // ugly qt stuff + void set_video_widget(bool val) { settings.video_widget = val; settings_changed(); } + void set_dyn_pose_res(bool val) { settings.dyn_pose_res = val; settings_changed(); } + void set_sleep_time(int val) { settings.sleep_time = val; settings_changed(); } + void set_reset_time(int val) { settings.reset_time = val; settings_changed(); } + void set_cam_index(int val) { settings.cam_index = val; settings_changed(); } + void set_cam_f(double val) { settings.cam_f = val; settings_changed(); } + void set_cam_res_x(int val) { settings.cam_res_x = val; settings_changed(); } + void set_cam_res_y(int val) { settings.cam_res_y = val; settings_changed(); } + void set_cam_fps(int val) { settings.cam_fps = val; settings_changed(); } + void set_cam_pitch(int val) { settings.cam_pitch = val; settings_changed(); } + void set_min_point_size(int val) { settings.min_point_size = val; settings_changed(); } + void set_max_point_size(int val) { settings.max_point_size = val; settings_changed(); } + void set_threshold(int val) { settings.threshold = val; settings_changed(); } + void set_ena_roll(bool val) { settings.bEnableRoll = val; settings_changed(); } + void set_ena_pitch(bool val) { settings.bEnablePitch = val; settings_changed(); } + void set_ena_yaw(bool val) { settings.bEnableYaw = val; settings_changed(); } + void set_ena_x(bool val) { settings.bEnableX = val; settings_changed(); } + void set_ena_y(bool val) { settings.bEnableY = val; settings_changed(); } + void set_ena_z(bool val) { settings.bEnableZ = val; settings_changed(); } + + void set_clip_t_height(int val) { dialog_settings.clip_ty = val; set_clip(); } + void set_clip_t_length(int val) { dialog_settings.clip_tz = val; set_clip(); } + void set_clip_b_height(int val) { dialog_settings.clip_by = val; set_clip(); } + void set_clip_b_length(int val) { dialog_settings.clip_bz = val; set_clip(); } + void set_cap_width(int val) { dialog_settings.cap_x = val; set_cap(); } + void set_cap_height(int val) { dialog_settings.cap_y = val; set_cap(); } + void set_cap_length(int val) { dialog_settings.cap_z = val; set_cap(); } + void set_m1x(int val) { dialog_settings.M01x = val; set_custom(); } + void set_m1y(int val) { dialog_settings.M01y = val; set_custom(); } + void set_m1z(int val) { dialog_settings.M01z = val; set_custom(); } + void set_m2x(int val) { dialog_settings.M02x = val; set_custom(); } + void set_m2y(int val) { dialog_settings.M02y = val; set_custom(); } + void set_m2z(int val) { dialog_settings.M02z = val; set_custom(); } + void set_tx(int val) { settings.t_MH[0] = val; settings_changed(); } + void set_ty(int val) { settings.t_MH[1] = val; settings_changed(); } + void set_tz(int val) { settings.t_MH[2] = val; settings_changed(); } + void set_model(int model_id); + + void doCenter(); + void doReset(); + + void doOK(); + void doCancel(); + + void startstop_trans_calib(bool start); + + void poll_tracker_info(); + +protected: + void set_clip(); + void set_cap(); + void set_custom(); + + void settings_changed(); + + TrackerSettings settings; + TrackerDialogSettings dialog_settings; + bool settings_dirty; + + Tracker* tracker; + TranslationCalibrator trans_calib; + bool trans_calib_running; + QTimer timer; + Ui::UICPTClientControls ui; +}; + +#endif //FTNOIR_TRACKER_PT_DIALOG_H diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp index 22c4a33d..1c486382 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp @@ -1,40 +1,40 @@ -/* 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. - */ - -#include "ftnoir_tracker_pt_dll.h" -#include -#include "facetracknoir/global-settings.h" - -//----------------------------------------------------------------------------- -void TrackerDll::getFullName(QString *strToBeFilled) -{ - *strToBeFilled = "PointTracker 1.0"; -} - -void TrackerDll::getShortName(QString *strToBeFilled) -{ - *strToBeFilled = "PointTracker"; -} - -void TrackerDll::getDescription(QString *strToBeFilled) -{ - *strToBeFilled = "Tracks a 3-point model with know geometry like Freetrack / TrackIR"; -} - -void TrackerDll::getIcon(QIcon *icon) -{ - *icon = QIcon(":/resources/icon.png"); -} - - -//----------------------------------------------------------------------------- -//#pragma comment(linker, "/export:GetTrackerDll=_GetTrackerDll@0") - -extern "C" FTNOIR_TRACKER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata() -{ - return new TrackerDll; -} +/* 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. + */ + +#include "ftnoir_tracker_pt_dll.h" +#include +#include "facetracknoir/global-settings.h" + +//----------------------------------------------------------------------------- +void TrackerDll::getFullName(QString *strToBeFilled) +{ + *strToBeFilled = "PointTracker 1.0"; +} + +void TrackerDll::getShortName(QString *strToBeFilled) +{ + *strToBeFilled = "PointTracker"; +} + +void TrackerDll::getDescription(QString *strToBeFilled) +{ + *strToBeFilled = "Tracks a 3-point model with know geometry like Freetrack / TrackIR"; +} + +void TrackerDll::getIcon(QIcon *icon) +{ + *icon = QIcon(":/resources/icon.png"); +} + + +//----------------------------------------------------------------------------- +//#pragma comment(linker, "/export:GetTrackerDll=_GetTrackerDll@0") + +extern "C" FTNOIR_TRACKER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata() +{ + return new TrackerDll; +} diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h index dde0bc9f..4dc94173 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h @@ -1,18 +1,18 @@ -/* 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. - */ - -#include "ftnoir_tracker_base/ftnoir_tracker_base.h" -#include "facetracknoir/global-settings.h" - -//----------------------------------------------------------------------------- -class TrackerDll : public Metadata -{ - void getFullName(QString *strToBeFilled); - void getShortName(QString *strToBeFilled); - void getDescription(QString *strToBeFilled); - void getIcon(QIcon *icon); -}; +/* 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. + */ + +#include "ftnoir_tracker_base/ftnoir_tracker_base.h" +#include "facetracknoir/global-settings.h" + +//----------------------------------------------------------------------------- +class TrackerDll : public Metadata +{ + void getFullName(QString *strToBeFilled); + void getShortName(QString *strToBeFilled); + void getDescription(QString *strToBeFilled); + void getIcon(QIcon *icon); +}; diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp index 3164b291..b0249813 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp @@ -1,150 +1,150 @@ -/* 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. - */ - -#include "ftnoir_tracker_pt.h" -#include -#include - -//----------------------------------------------------------------------------- -void TrackerSettings::load_ini() -{ - qDebug("TrackerSettings::load_ini()"); - - QSettings settings("opentrack"); // Registry settings (in HK_USER) - QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) - - iniFile.beginGroup( "PointTracker" ); - - cam_index = iniFile.value("CameraId", 0).toInt(); - cam_f = iniFile.value("CameraF", 1).toFloat(); - cam_res_x = iniFile.value("CameraResX", 640).toInt(); - cam_res_y = iniFile.value("CameraResY", 480).toInt(); - cam_fps = iniFile.value("CameraFPS", 30).toInt(); - cam_pitch = iniFile.value("CameraPitch", 0).toInt(); - threshold = iniFile.value("PointExtractThreshold", 128).toInt(); - min_point_size = iniFile.value("PointExtractMinSize", 2).toInt(); - max_point_size = iniFile.value("PointExtractMaxSize", 50).toInt(); - M01[0] = iniFile.value("PointModelM01x", 0).toFloat(); - M01[1] = iniFile.value("PointModelM01y", 40).toFloat(); - M01[2] = iniFile.value("PointModelM01z", -30).toFloat(); - M02[0] = iniFile.value("PointModelM02x", 0).toFloat(); - M02[1] = iniFile.value("PointModelM02y", -70).toFloat(); - M02[2] = iniFile.value("PointModelM02z", -80).toFloat(); - t_MH[0] = iniFile.value("tMHx", 0).toFloat(); - t_MH[1] = iniFile.value("tMHy", 0).toFloat(); - t_MH[2] = iniFile.value("tMHz", 0).toFloat(); - dyn_pose_res = iniFile.value("DynamicPoseResolution", true).toBool(); - video_widget = iniFile.value("VideoWidget", true).toBool(); - sleep_time = iniFile.value("SleepTime", 10).toInt(); - reset_time = iniFile.value("ResetTime", 1000).toInt(); - - bEnableRoll = iniFile.value( "EnableRoll", 1 ).toBool(); - bEnablePitch = iniFile.value( "EnablePitch", 1 ).toBool(); - bEnableYaw = iniFile.value( "EnableYaw", 1 ).toBool(); - bEnableX = iniFile.value( "EnableX", 1 ).toBool(); - bEnableY = iniFile.value( "EnableY", 1 ).toBool(); - bEnableZ = iniFile.value( "EnableZ", 1 ).toBool(); - - iniFile.endGroup(); -} - -void TrackerSettings::save_ini() const -{ - qDebug("TrackerSettings::save_ini()"); - - QSettings settings("opentrack"); // Registry settings (in HK_USER) - QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) - - iniFile.beginGroup ( "PointTracker" ); - - iniFile.setValue("CameraId", cam_index); - iniFile.setValue("CameraF", cam_f); - iniFile.setValue("CameraResX", cam_res_x); - iniFile.setValue("CameraResY", cam_res_y); - iniFile.setValue("CameraFPS", cam_fps); - iniFile.setValue("CameraPitch", cam_pitch); - iniFile.setValue("PointExtractThreshold", threshold); - iniFile.setValue("PointExtractMinSize", min_point_size); - iniFile.setValue("PointExtractMaxSize", max_point_size); - iniFile.setValue("PointModelM01x", M01[0]); - iniFile.setValue("PointModelM01y", M01[1]); - iniFile.setValue("PointModelM01z", M01[2]); - iniFile.setValue("PointModelM02x", M02[0]); - iniFile.setValue("PointModelM02y", M02[1]); - iniFile.setValue("PointModelM02z", M02[2]); - iniFile.setValue("tMHx", t_MH[0]); - iniFile.setValue("tMHy", t_MH[1]); - iniFile.setValue("tMHz", t_MH[2]); - iniFile.setValue("DynamicPoseResolution", dyn_pose_res); - iniFile.setValue("VideoWidget", video_widget); - iniFile.setValue("SleepTime", sleep_time); - iniFile.setValue("ResetTime", reset_time); - - iniFile.setValue( "EnableRoll", bEnableRoll ); - iniFile.setValue( "EnablePitch", bEnablePitch ); - iniFile.setValue( "EnableYaw", bEnableYaw ); - iniFile.setValue( "EnableX", bEnableX ); - iniFile.setValue( "EnableY", bEnableY ); - iniFile.setValue( "EnableZ", bEnableZ ); - - iniFile.endGroup(); -} - -//----------------------------------------------------------------------------- -void TrackerDialogSettings::load_ini() -{ - qDebug("TrackerDialogSettings::load_ini()"); - - QSettings settings("opentrack"); // Registry settings (in HK_USER) - QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) - - iniFile.beginGroup( "PointTrackerDialog" ); - - active_model_panel = iniFile.value("ActiveModelPanel", MODEL_CLIP).toInt(); - M01x = iniFile.value("CustomM01x", 0).toInt(); - M01y = iniFile.value("CustomM01y", 40).toInt(); - M01z = iniFile.value("CustomM01z", -30).toInt(); - M02x = iniFile.value("CustomM02x", 0).toInt(); - M02y = iniFile.value("CustomM02y", -70).toInt(); - M02z = iniFile.value("CustomM02z", -80).toInt(); - clip_ty = iniFile.value("ClipTopHeight", 40).toInt(); - clip_tz = iniFile.value("ClipTopLength", 30).toInt(); - clip_by = iniFile.value("ClipBottomHeight", 70).toInt(); - clip_bz = iniFile.value("ClipBottomLength", 80).toInt(); - cap_x = iniFile.value("CapHalfWidth", 40).toInt(); - cap_y = iniFile.value("CapHeight", 60).toInt(); - cap_z = iniFile.value("CapLength", 100).toInt(); -} - -void TrackerDialogSettings::save_ini() const -{ - qDebug("TrackerDialogSettings::save_ini()"); - - QSettings settings("opentrack"); // Registry settings (in HK_USER) - QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) - - iniFile.beginGroup ( "PointTrackerDialog" ); - - iniFile.setValue("ActiveModelPanel", active_model_panel); - iniFile.setValue("CustomM01x", M01x); - iniFile.setValue("CustomM01y", M01y); - iniFile.setValue("CustomM01z", M01z); - iniFile.setValue("CustomM02x", M02x); - iniFile.setValue("CustomM02y", M02y); - iniFile.setValue("CustomM02z", M02z); - iniFile.setValue("ClipTopHeight", clip_ty); - iniFile.setValue("ClipTopLength", clip_tz); - iniFile.setValue("ClipBottomHeight", clip_by); - iniFile.setValue("ClipBottomLength", clip_bz); - iniFile.setValue("CapHalfWidth", cap_x); - iniFile.setValue("CapHeight", cap_y); - iniFile.setValue("CapLength", cap_z); +/* 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. + */ + +#include "ftnoir_tracker_pt.h" +#include +#include + +//----------------------------------------------------------------------------- +void TrackerSettings::load_ini() +{ + qDebug("TrackerSettings::load_ini()"); + + QSettings settings("opentrack"); // Registry settings (in HK_USER) + QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup( "PointTracker" ); + + cam_index = iniFile.value("CameraId", 0).toInt(); + cam_f = iniFile.value("CameraF", 1).toFloat(); + cam_res_x = iniFile.value("CameraResX", 640).toInt(); + cam_res_y = iniFile.value("CameraResY", 480).toInt(); + cam_fps = iniFile.value("CameraFPS", 30).toInt(); + cam_pitch = iniFile.value("CameraPitch", 0).toInt(); + threshold = iniFile.value("PointExtractThreshold", 128).toInt(); + min_point_size = iniFile.value("PointExtractMinSize", 2).toInt(); + max_point_size = iniFile.value("PointExtractMaxSize", 50).toInt(); + M01[0] = iniFile.value("PointModelM01x", 0).toFloat(); + M01[1] = iniFile.value("PointModelM01y", 40).toFloat(); + M01[2] = iniFile.value("PointModelM01z", -30).toFloat(); + M02[0] = iniFile.value("PointModelM02x", 0).toFloat(); + M02[1] = iniFile.value("PointModelM02y", -70).toFloat(); + M02[2] = iniFile.value("PointModelM02z", -80).toFloat(); + t_MH[0] = iniFile.value("tMHx", 0).toFloat(); + t_MH[1] = iniFile.value("tMHy", 0).toFloat(); + t_MH[2] = iniFile.value("tMHz", 0).toFloat(); + dyn_pose_res = iniFile.value("DynamicPoseResolution", true).toBool(); + video_widget = iniFile.value("VideoWidget", true).toBool(); + sleep_time = iniFile.value("SleepTime", 10).toInt(); + reset_time = iniFile.value("ResetTime", 1000).toInt(); + + bEnableRoll = iniFile.value( "EnableRoll", 1 ).toBool(); + bEnablePitch = iniFile.value( "EnablePitch", 1 ).toBool(); + bEnableYaw = iniFile.value( "EnableYaw", 1 ).toBool(); + bEnableX = iniFile.value( "EnableX", 1 ).toBool(); + bEnableY = iniFile.value( "EnableY", 1 ).toBool(); + bEnableZ = iniFile.value( "EnableZ", 1 ).toBool(); + + iniFile.endGroup(); +} + +void TrackerSettings::save_ini() const +{ + qDebug("TrackerSettings::save_ini()"); + + QSettings settings("opentrack"); // Registry settings (in HK_USER) + QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup ( "PointTracker" ); + + iniFile.setValue("CameraId", cam_index); + iniFile.setValue("CameraF", cam_f); + iniFile.setValue("CameraResX", cam_res_x); + iniFile.setValue("CameraResY", cam_res_y); + iniFile.setValue("CameraFPS", cam_fps); + iniFile.setValue("CameraPitch", cam_pitch); + iniFile.setValue("PointExtractThreshold", threshold); + iniFile.setValue("PointExtractMinSize", min_point_size); + iniFile.setValue("PointExtractMaxSize", max_point_size); + iniFile.setValue("PointModelM01x", M01[0]); + iniFile.setValue("PointModelM01y", M01[1]); + iniFile.setValue("PointModelM01z", M01[2]); + iniFile.setValue("PointModelM02x", M02[0]); + iniFile.setValue("PointModelM02y", M02[1]); + iniFile.setValue("PointModelM02z", M02[2]); + iniFile.setValue("tMHx", t_MH[0]); + iniFile.setValue("tMHy", t_MH[1]); + iniFile.setValue("tMHz", t_MH[2]); + iniFile.setValue("DynamicPoseResolution", dyn_pose_res); + iniFile.setValue("VideoWidget", video_widget); + iniFile.setValue("SleepTime", sleep_time); + iniFile.setValue("ResetTime", reset_time); + + iniFile.setValue( "EnableRoll", bEnableRoll ); + iniFile.setValue( "EnablePitch", bEnablePitch ); + iniFile.setValue( "EnableYaw", bEnableYaw ); + iniFile.setValue( "EnableX", bEnableX ); + iniFile.setValue( "EnableY", bEnableY ); + iniFile.setValue( "EnableZ", bEnableZ ); + + iniFile.endGroup(); +} + +//----------------------------------------------------------------------------- +void TrackerDialogSettings::load_ini() +{ + qDebug("TrackerDialogSettings::load_ini()"); + + QSettings settings("opentrack"); // Registry settings (in HK_USER) + QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup( "PointTrackerDialog" ); + + active_model_panel = iniFile.value("ActiveModelPanel", MODEL_CLIP).toInt(); + M01x = iniFile.value("CustomM01x", 0).toInt(); + M01y = iniFile.value("CustomM01y", 40).toInt(); + M01z = iniFile.value("CustomM01z", -30).toInt(); + M02x = iniFile.value("CustomM02x", 0).toInt(); + M02y = iniFile.value("CustomM02y", -70).toInt(); + M02z = iniFile.value("CustomM02z", -80).toInt(); + clip_ty = iniFile.value("ClipTopHeight", 40).toInt(); + clip_tz = iniFile.value("ClipTopLength", 30).toInt(); + clip_by = iniFile.value("ClipBottomHeight", 70).toInt(); + clip_bz = iniFile.value("ClipBottomLength", 80).toInt(); + cap_x = iniFile.value("CapHalfWidth", 40).toInt(); + cap_y = iniFile.value("CapHeight", 60).toInt(); + cap_z = iniFile.value("CapLength", 100).toInt(); +} + +void TrackerDialogSettings::save_ini() const +{ + qDebug("TrackerDialogSettings::save_ini()"); + + QSettings settings("opentrack"); // Registry settings (in HK_USER) + QString currentFile = settings.value( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + iniFile.beginGroup ( "PointTrackerDialog" ); + + iniFile.setValue("ActiveModelPanel", active_model_panel); + iniFile.setValue("CustomM01x", M01x); + iniFile.setValue("CustomM01y", M01y); + iniFile.setValue("CustomM01z", M01z); + iniFile.setValue("CustomM02x", M02x); + iniFile.setValue("CustomM02y", M02y); + iniFile.setValue("CustomM02z", M02z); + iniFile.setValue("ClipTopHeight", clip_ty); + iniFile.setValue("ClipTopLength", clip_tz); + iniFile.setValue("ClipBottomHeight", clip_by); + iniFile.setValue("ClipBottomLength", clip_bz); + iniFile.setValue("CapHalfWidth", cap_x); + iniFile.setValue("CapHeight", cap_y); + iniFile.setValue("CapLength", cap_z); } \ No newline at end of file diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h index 88162c86..a1523898 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h @@ -1,84 +1,84 @@ -/* 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. - */ - -#ifndef FTNOIR_TRACKER_PT_SETTINGS_H -#define FTNOIR_TRACKER_PT_SETTINGS_H - -#include -#include "point_tracker.h" - - -//----------------------------------------------------------------------------- -struct TrackerSettings -{ - // camera - int cam_index; - float cam_f; - int cam_res_x; - int cam_res_y; - int cam_fps; - int cam_pitch; - - // point extraction - int threshold; - int min_point_size; - int max_point_size; - - // point tracking - cv::Vec3f M01; - cv::Vec3f M02; - bool dyn_pose_res; - - // head to model translation - cv::Vec3f t_MH; - - int sleep_time; // in ms - int reset_time; // in ms - bool video_widget; - - bool bEnableRoll; - bool bEnablePitch; - bool bEnableYaw; - bool bEnableX; - bool bEnableY; - bool bEnableZ; - - void load_ini(); - void save_ini() const; -}; - - -//----------------------------------------------------------------------------- -struct TrackerDialogSettings -{ - enum - { - MODEL_CLIP, - MODEL_CAP, - MODEL_CUSTOM - }; - int active_model_panel; - - int M01x; - int M01y; - int M01z; - int M02x; - int M02y; - int M02z; - int clip_ty; - int clip_tz; - int clip_by; - int clip_bz; - int cap_x; - int cap_y; - int cap_z; - - void load_ini(); - void save_ini() const; -}; - +/* 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. + */ + +#ifndef FTNOIR_TRACKER_PT_SETTINGS_H +#define FTNOIR_TRACKER_PT_SETTINGS_H + +#include +#include "point_tracker.h" + + +//----------------------------------------------------------------------------- +struct TrackerSettings +{ + // camera + int cam_index; + float cam_f; + int cam_res_x; + int cam_res_y; + int cam_fps; + int cam_pitch; + + // point extraction + int threshold; + int min_point_size; + int max_point_size; + + // point tracking + cv::Vec3f M01; + cv::Vec3f M02; + bool dyn_pose_res; + + // head to model translation + cv::Vec3f t_MH; + + int sleep_time; // in ms + int reset_time; // in ms + bool video_widget; + + bool bEnableRoll; + bool bEnablePitch; + bool bEnableYaw; + bool bEnableX; + bool bEnableY; + bool bEnableZ; + + void load_ini(); + void save_ini() const; +}; + + +//----------------------------------------------------------------------------- +struct TrackerDialogSettings +{ + enum + { + MODEL_CLIP, + MODEL_CAP, + MODEL_CUSTOM + }; + int active_model_panel; + + int M01x; + int M01y; + int M01z; + int M02x; + int M02y; + int M02z; + int clip_ty; + int clip_tz; + int clip_by; + int clip_bz; + int cap_x; + int cap_y; + int cap_z; + + void load_ini(); + void save_ini() const; +}; + #endif //FTNOIR_TRACKER_PT_SETTINGS_H \ No newline at end of file diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index 61e86bec..09be967a 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -1,96 +1,96 @@ -/* 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. - */ - -#include "point_extractor.h" -#include - -using namespace cv; -using namespace std; - -// ---------------------------------------------------------------------------- -const vector& PointExtractor::extract_points(Mat frame, float dt, bool draw_output) -{ - const int W = frame.cols; - const int H = frame.rows; - - // clear old points - points.clear(); - - // convert to grayscale - Mat frame_grey; - cvtColor(frame, frame_grey, COLOR_BGR2GRAY); - - // convert to binary - Mat frame_bin; - threshold(frame_grey, frame_bin, threshold_val, 255, THRESH_BINARY); - - unsigned int region_size_min = 3.14*min_size*min_size; - unsigned int region_size_max = 3.14*max_size*max_size; - - int blob_index = 1; - for (int y=0; y(y,x) != 255) continue; - Rect rect; - floodFill(frame_bin, Point(x,y), Scalar(blob_index), &rect, Scalar(0), Scalar(0), FLOODFILL_FIXED_RANGE); - blob_index++; - - // calculate the size of the connected component - unsigned int region_size = 0; - for (int i=rect.y; i < (rect.y+rect.height); i++) - { - for (int j=rect.x; j < (rect.x+rect.width); j++) - { - if (frame_bin.at(i,j) != blob_index-1) continue; - region_size++; - } - } - - if (region_size < region_size_min || region_size > region_size_max) continue; - - // calculate the center of mass: - // mx = (sum_ij j*f(frame_grey_ij)) / (sum_ij f(frame_grey_ij)) - // my = ... - // f maps from [threshold,256] -> [0, 1], lower values are mapped to 0 - float m = 0; - float mx = 0; - float my = 0; - for (int i=rect.y; i < (rect.y+rect.height); i++) - { - for (int j=rect.x; j < (rect.x+rect.width); j++) - { - if (frame_bin.at(i,j) != blob_index-1) continue; - float val = frame_grey.at(i,j); - val = float(val - threshold_val)/(256 - threshold_val); - val = val*val; // makes it more stable (less emphasis on low values, more on the peak) - m += val; - mx += j * val; - my += i * val; - } - } - - // convert to centered camera coordinate system with y axis upwards - Vec2f c; - c[0] = (mx/m - W/2)/W; - c[1] = -(my/m - H/2)/W; - points.push_back(c); - - if (blob_index >= 255) break; - } - if (blob_index >= 255) break; - } - - // draw output image - if (draw_output) { - frame.setTo(Scalar(255,0,0), frame_bin); - } - - return points; -} +/* 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. + */ + +#include "point_extractor.h" +#include + +using namespace cv; +using namespace std; + +// ---------------------------------------------------------------------------- +const vector& PointExtractor::extract_points(Mat frame, float dt, bool draw_output) +{ + const int W = frame.cols; + const int H = frame.rows; + + // clear old points + points.clear(); + + // convert to grayscale + Mat frame_grey; + cvtColor(frame, frame_grey, COLOR_BGR2GRAY); + + // convert to binary + Mat frame_bin; + threshold(frame_grey, frame_bin, threshold_val, 255, THRESH_BINARY); + + unsigned int region_size_min = 3.14*min_size*min_size; + unsigned int region_size_max = 3.14*max_size*max_size; + + int blob_index = 1; + for (int y=0; y(y,x) != 255) continue; + Rect rect; + floodFill(frame_bin, Point(x,y), Scalar(blob_index), &rect, Scalar(0), Scalar(0), FLOODFILL_FIXED_RANGE); + blob_index++; + + // calculate the size of the connected component + unsigned int region_size = 0; + for (int i=rect.y; i < (rect.y+rect.height); i++) + { + for (int j=rect.x; j < (rect.x+rect.width); j++) + { + if (frame_bin.at(i,j) != blob_index-1) continue; + region_size++; + } + } + + if (region_size < region_size_min || region_size > region_size_max) continue; + + // calculate the center of mass: + // mx = (sum_ij j*f(frame_grey_ij)) / (sum_ij f(frame_grey_ij)) + // my = ... + // f maps from [threshold,256] -> [0, 1], lower values are mapped to 0 + float m = 0; + float mx = 0; + float my = 0; + for (int i=rect.y; i < (rect.y+rect.height); i++) + { + for (int j=rect.x; j < (rect.x+rect.width); j++) + { + if (frame_bin.at(i,j) != blob_index-1) continue; + float val = frame_grey.at(i,j); + val = float(val - threshold_val)/(256 - threshold_val); + val = val*val; // makes it more stable (less emphasis on low values, more on the peak) + m += val; + mx += j * val; + my += i * val; + } + } + + // convert to centered camera coordinate system with y axis upwards + Vec2f c; + c[0] = (mx/m - W/2)/W; + c[1] = -(my/m - H/2)/W; + points.push_back(c); + + if (blob_index >= 255) break; + } + if (blob_index >= 255) break; + } + + // draw output image + if (draw_output) { + frame.setTo(Scalar(255,0,0), frame_bin); + } + + return points; +} diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h index b142d2bb..9a6f7f2c 100644 --- a/ftnoir_tracker_pt/point_extractor.h +++ b/ftnoir_tracker_pt/point_extractor.h @@ -1,31 +1,31 @@ -/* 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. - */ - -#ifndef POINTEXTRACTOR_H -#define POINTEXTRACTOR_H - -#include - -// ---------------------------------------------------------------------------- -// Extracts points from an opencv image -class PointExtractor -{ -public: - // extracts points from frame and draws some processing info into frame, if draw_output is set - // dt: time since last call in seconds - // WARNING: returned reference is valid as long as object - const std::vector& extract_points(cv::Mat frame, float dt, bool draw_output); - const std::vector& get_points() { return points; } - - int threshold_val; - int min_size, max_size; - -protected: - std::vector points; -}; - -#endif //POINTEXTRACTOR_H +/* 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. + */ + +#ifndef POINTEXTRACTOR_H +#define POINTEXTRACTOR_H + +#include + +// ---------------------------------------------------------------------------- +// Extracts points from an opencv image +class PointExtractor +{ +public: + // extracts points from frame and draws some processing info into frame, if draw_output is set + // dt: time since last call in seconds + // WARNING: returned reference is valid as long as object + const std::vector& extract_points(cv::Mat frame, float dt, bool draw_output); + const std::vector& get_points() { return points; } + + int threshold_val; + int min_size, max_size; + +protected: + std::vector points; +}; + +#endif //POINTEXTRACTOR_H diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp index c08d6d83..210ed2eb 100644 --- a/ftnoir_tracker_pt/point_tracker.cpp +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -1,352 +1,352 @@ -/* 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. - */ - -#include "point_tracker.h" - -#include -#include -#include - -#include - -using namespace cv; -using namespace std; - -const float PI = 3.14159265358979323846f; - -// ---------------------------------------------------------------------------- -static void get_row(const Matx33f& m, int i, Vec3f& v) -{ - v[0] = m(i,0); - v[1] = m(i,1); - v[2] = m(i,2); -} - -static void set_row(Matx33f& m, int i, const Vec3f& v) -{ - m(i,0) = v[0]; - m(i,1) = v[1]; - m(i,2) = v[2]; -} - -// ---------------------------------------------------------------------------- -PointModel::PointModel(Vec3f M01, Vec3f M02) - : M01(M01), M02(M02) -{ - // calculate u - u = M01.cross(M02); - u /= norm(u); - - // calculate projection matrix on M01,M02 plane - float s11 = M01.dot(M01); - float s12 = M01.dot(M02); - float s22 = M02.dot(M02); - P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, - -s12, s11); - - // calculate d and d_order for simple freetrack-like point correspondence - vector points; - points.push_back(Vec2f(0,0)); - points.push_back(Vec2f(M01[0], M01[1])); - points.push_back(Vec2f(M02[0], M02[1])); - // fit line to orthographically projected points - // ERROR: yields wrong results with colinear points?! - /* - Vec4f line; - fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01); - d[0] = line[0]; d[1] = line[1]; - */ - // TODO: fix this - d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]); - - // sort model points - get_d_order(points, d_order); -} - -static bool d_vals_sort(const pair a, const pair b) -{ - return a.first < b.first; -} - -void PointModel::get_d_order(const std::vector& points, int d_order[]) const -{ - // get sort indices with respect to d scalar product - vector< pair > d_vals; - for (int i = 0; i<(int)points.size(); ++i) - d_vals.push_back(pair(d.dot(points[i]), i)); - - sort(d_vals.begin(), d_vals.end(), d_vals_sort); - - for (int i = 0; i<(int)points.size(); ++i) - d_order[i] = d_vals[i].second; -} - - -// ---------------------------------------------------------------------------- -PointTracker::PointTracker() : dynamic_pose_resolution(true), dt_reset(1), init_phase(true), dt_valid(0), v_t(0,0,0), v_r(0,0,0) -{ - X_CM.t[2] = 1000; // default position: 1 m away from cam; -} - -void PointTracker::reset() -{ - // enter init phase and reset velocities - init_phase = true; - dt_valid = 0; - reset_velocities(); -} - -void PointTracker::reset_velocities() -{ - v_t = Vec3f(0,0,0); - v_r = Vec3f(0,0,0); -} - - -bool PointTracker::track(const vector& points, float f, float dt) -{ - if (!dynamic_pose_resolution) init_phase = true; - - dt_valid += dt; - // if there was no valid tracking result for too long, do a reset - if (dt_valid > dt_reset) - { - //qDebug()<<"dt_valid "< dt_reset "<& points, float f) -{ - if (init_phase) { - // We do a simple freetrack-like sorting in the init phase... - // sort points - int point_d_order[PointModel::N_POINTS]; - point_model->get_d_order(points, point_d_order); - - // set correspondences - for (int i=0; id_order[i]] = points[point_d_order[i]]; - } - } - else { - // ... otherwise we look at the distance to the projection of the expected model points - // project model points under current pose - p_exp[0] = project(Vec3f(0,0,0), f); - p_exp[1] = project(point_model->M01, f); - p_exp[2] = project(point_model->M02, f); - - // set correspondences by minimum distance to projected model point - bool point_taken[PointModel::N_POINTS]; - for (int i=0; iM01)/Z0; - epsilon_2 = k.dot(point_model->M02)/Z0; - - // vector of scalar products and - Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], - p[2][0]*(1.0 + epsilon_2) - p[0][0]); - Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1], - p[2][1]*(1.0 + epsilon_2) - p[0][1]); - - // construct projection of I, J onto M0i plane: I0 and J0 - I0_coeff = point_model->P * I0_M0i; - J0_coeff = point_model->P * J0_M0i; - I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02; - J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02; - - // calculate u component of I, J - float II0 = I0.dot(I0); - float IJ0 = I0.dot(J0); - float JJ0 = J0.dot(J0); - float rho, theta; - if (JJ0 == II0) { - rho = sqrt(abs(2*IJ0)); - theta = -PI/4; - if (IJ0<0) theta *= -1; - } - else { - rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 )); - theta = atan( -2*IJ0 / (JJ0-II0) ); - if (JJ0 - II0 < 0) theta += PI; - theta /= 2; - } - - // construct the two solutions - I_1 = I0 + rho*cos(theta)*point_model->u; - I_2 = I0 - rho*cos(theta)*point_model->u; - - J_1 = J0 + rho*sin(theta)*point_model->u; - J_2 = J0 - rho*sin(theta)*point_model->u; - - float norm_const = 1.0/norm(I_1); // all have the same norm - - // create rotation matrices - I_1 *= norm_const; J_1 *= norm_const; - I_2 *= norm_const; J_2 *= norm_const; - - set_row(R_1, 0, I_1); - set_row(R_1, 1, J_1); - set_row(R_1, 2, I_1.cross(J_1)); - - set_row(R_2, 0, I_2); - set_row(R_2, 1, J_2); - set_row(R_2, 2, I_2.cross(J_2)); - - // the single translation solution - Z0 = norm_const * f; - - // pick the rotation solution closer to the expected one - // in simple metric d(A,B) = || I - A * B^T || - float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t()); - float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t()); - - if (R_1_deviation < R_2_deviation) - R_current = &R_1; - else - R_current = &R_2; - - get_row(*R_current, 2, k); - - // check for convergence condition - if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) - break; - old_epsilon_1 = epsilon_1; - old_epsilon_2 = epsilon_2; - } - - // apply results - X_CM.R = *R_current; - X_CM.t[0] = p[0][0] * Z0/f; - X_CM.t[1] = p[0][1] * Z0/f; - X_CM.t[2] = Z0; - - return i; - - //Rodrigues(X_CM.R, r); - //qDebug()<<"iter: "< +#include +#include + +#include + +using namespace cv; +using namespace std; + +const float PI = 3.14159265358979323846f; + +// ---------------------------------------------------------------------------- +static void get_row(const Matx33f& m, int i, Vec3f& v) +{ + v[0] = m(i,0); + v[1] = m(i,1); + v[2] = m(i,2); +} + +static void set_row(Matx33f& m, int i, const Vec3f& v) +{ + m(i,0) = v[0]; + m(i,1) = v[1]; + m(i,2) = v[2]; +} + +// ---------------------------------------------------------------------------- +PointModel::PointModel(Vec3f M01, Vec3f M02) + : M01(M01), M02(M02) +{ + // calculate u + u = M01.cross(M02); + u /= norm(u); + + // calculate projection matrix on M01,M02 plane + float s11 = M01.dot(M01); + float s12 = M01.dot(M02); + float s22 = M02.dot(M02); + P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, + -s12, s11); + + // calculate d and d_order for simple freetrack-like point correspondence + vector points; + points.push_back(Vec2f(0,0)); + points.push_back(Vec2f(M01[0], M01[1])); + points.push_back(Vec2f(M02[0], M02[1])); + // fit line to orthographically projected points + // ERROR: yields wrong results with colinear points?! + /* + Vec4f line; + fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01); + d[0] = line[0]; d[1] = line[1]; + */ + // TODO: fix this + d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]); + + // sort model points + get_d_order(points, d_order); +} + +static bool d_vals_sort(const pair a, const pair b) +{ + return a.first < b.first; +} + +void PointModel::get_d_order(const std::vector& points, int d_order[]) const +{ + // get sort indices with respect to d scalar product + vector< pair > d_vals; + for (int i = 0; i<(int)points.size(); ++i) + d_vals.push_back(pair(d.dot(points[i]), i)); + + sort(d_vals.begin(), d_vals.end(), d_vals_sort); + + for (int i = 0; i<(int)points.size(); ++i) + d_order[i] = d_vals[i].second; +} + + +// ---------------------------------------------------------------------------- +PointTracker::PointTracker() : dynamic_pose_resolution(true), dt_reset(1), init_phase(true), dt_valid(0), v_t(0,0,0), v_r(0,0,0) +{ + X_CM.t[2] = 1000; // default position: 1 m away from cam; +} + +void PointTracker::reset() +{ + // enter init phase and reset velocities + init_phase = true; + dt_valid = 0; + reset_velocities(); +} + +void PointTracker::reset_velocities() +{ + v_t = Vec3f(0,0,0); + v_r = Vec3f(0,0,0); +} + + +bool PointTracker::track(const vector& points, float f, float dt) +{ + if (!dynamic_pose_resolution) init_phase = true; + + dt_valid += dt; + // if there was no valid tracking result for too long, do a reset + if (dt_valid > dt_reset) + { + //qDebug()<<"dt_valid "< dt_reset "<& points, float f) +{ + if (init_phase) { + // We do a simple freetrack-like sorting in the init phase... + // sort points + int point_d_order[PointModel::N_POINTS]; + point_model->get_d_order(points, point_d_order); + + // set correspondences + for (int i=0; id_order[i]] = points[point_d_order[i]]; + } + } + else { + // ... otherwise we look at the distance to the projection of the expected model points + // project model points under current pose + p_exp[0] = project(Vec3f(0,0,0), f); + p_exp[1] = project(point_model->M01, f); + p_exp[2] = project(point_model->M02, f); + + // set correspondences by minimum distance to projected model point + bool point_taken[PointModel::N_POINTS]; + for (int i=0; iM01)/Z0; + epsilon_2 = k.dot(point_model->M02)/Z0; + + // vector of scalar products and + Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], + p[2][0]*(1.0 + epsilon_2) - p[0][0]); + Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1], + p[2][1]*(1.0 + epsilon_2) - p[0][1]); + + // construct projection of I, J onto M0i plane: I0 and J0 + I0_coeff = point_model->P * I0_M0i; + J0_coeff = point_model->P * J0_M0i; + I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02; + J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02; + + // calculate u component of I, J + float II0 = I0.dot(I0); + float IJ0 = I0.dot(J0); + float JJ0 = J0.dot(J0); + float rho, theta; + if (JJ0 == II0) { + rho = sqrt(abs(2*IJ0)); + theta = -PI/4; + if (IJ0<0) theta *= -1; + } + else { + rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 )); + theta = atan( -2*IJ0 / (JJ0-II0) ); + if (JJ0 - II0 < 0) theta += PI; + theta /= 2; + } + + // construct the two solutions + I_1 = I0 + rho*cos(theta)*point_model->u; + I_2 = I0 - rho*cos(theta)*point_model->u; + + J_1 = J0 + rho*sin(theta)*point_model->u; + J_2 = J0 - rho*sin(theta)*point_model->u; + + float norm_const = 1.0/norm(I_1); // all have the same norm + + // create rotation matrices + I_1 *= norm_const; J_1 *= norm_const; + I_2 *= norm_const; J_2 *= norm_const; + + set_row(R_1, 0, I_1); + set_row(R_1, 1, J_1); + set_row(R_1, 2, I_1.cross(J_1)); + + set_row(R_2, 0, I_2); + set_row(R_2, 1, J_2); + set_row(R_2, 2, I_2.cross(J_2)); + + // the single translation solution + Z0 = norm_const * f; + + // pick the rotation solution closer to the expected one + // in simple metric d(A,B) = || I - A * B^T || + float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t()); + float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t()); + + if (R_1_deviation < R_2_deviation) + R_current = &R_1; + else + R_current = &R_2; + + get_row(*R_current, 2, k); + + // check for convergence condition + if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) + break; + old_epsilon_1 = epsilon_1; + old_epsilon_2 = epsilon_2; + } + + // apply results + X_CM.R = *R_current; + X_CM.t[0] = p[0][0] * Z0/f; + X_CM.t[1] = p[0][1] * Z0/f; + X_CM.t[2] = Z0; + + return i; + + //Rodrigues(X_CM.R, r); + //qDebug()<<"iter: "< -#include -#include - -// ---------------------------------------------------------------------------- -// Afine frame trafo -class FrameTrafo -{ -public: - FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} - FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} - - cv::Matx33f R; - cv::Vec3f t; -}; - -inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) -{ - return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); -} - -inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) -{ - return X.R*v + X.t; -} - -// ---------------------------------------------------------------------------- -// Describes a 3-point model -// nomenclature as in -// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] -class PointModel -{ - friend class PointTracker; -public: - static const int N_POINTS = 3; - - PointModel(cv::Vec3f M01, cv::Vec3f M02); - - const cv::Vec3f& get_M01() const { return M01; }; - const cv::Vec3f& get_M02() const { return M02; }; - -protected: - cv::Vec3f M01; // M01 in model frame - cv::Vec3f M02; // M02 in model frame - - cv::Vec3f u; // unit vector perpendicular to M01,M02-plane - - cv::Matx22f P; - - cv::Vec2f d; // discrimant vector for point correspondence - int d_order[3]; // sorting of projected model points with respect to d scalar product - - void get_d_order(const std::vector& points, int d_order[]) const; -}; - -// ---------------------------------------------------------------------------- -// Tracks a 3-point model -// implementing the POSIT algorithm for coplanar points as presented in -// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] -class PointTracker -{ -public: - PointTracker(); - - // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) - // f : (focal length)/(sensor width) - // dt : time since last call - bool track(const std::vector& points, float f, float dt); - std::auto_ptr point_model; - - bool dynamic_pose_resolution; - float dt_reset; - - FrameTrafo get_pose() const { return X_CM; } - void reset(); - -protected: - inline cv::Vec2f project(const cv::Vec3f& v_M, float f) - { - cv::Vec3f v_C = X_CM * v_M; - return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); - } - - bool find_correspondences(const std::vector& points, float f); - - cv::Vec2f p[PointModel::N_POINTS]; // the points in model order - cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions - - void predict(float dt); - void update_velocities(float dt); - void reset_velocities(); - - - int POSIT(float f); // The POSIT algorithm, returns the number of iterations - - bool init_phase; - float dt_valid; // time since last valid tracking result - cv::Vec3f v_t; // velocities - cv::Vec3f v_r; - FrameTrafo X_CM; // trafo from model to camera - FrameTrafo X_CM_old; -}; - -#endif //POINTTRACKER_H +/* 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. + */ + +#ifndef POINTTRACKER_H +#define POINTTRACKER_H + +#include +#include +#include + +// ---------------------------------------------------------------------------- +// Afine frame trafo +class FrameTrafo +{ +public: + FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} + FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} + + cv::Matx33f R; + cv::Vec3f t; +}; + +inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) +{ + return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); +} + +inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) +{ + return X.R*v + X.t; +} + +// ---------------------------------------------------------------------------- +// Describes a 3-point model +// nomenclature as in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] +class PointModel +{ + friend class PointTracker; +public: + static const int N_POINTS = 3; + + PointModel(cv::Vec3f M01, cv::Vec3f M02); + + const cv::Vec3f& get_M01() const { return M01; }; + const cv::Vec3f& get_M02() const { return M02; }; + +protected: + cv::Vec3f M01; // M01 in model frame + cv::Vec3f M02; // M02 in model frame + + cv::Vec3f u; // unit vector perpendicular to M01,M02-plane + + cv::Matx22f P; + + cv::Vec2f d; // discrimant vector for point correspondence + int d_order[3]; // sorting of projected model points with respect to d scalar product + + void get_d_order(const std::vector& points, int d_order[]) const; +}; + +// ---------------------------------------------------------------------------- +// Tracks a 3-point model +// implementing the POSIT algorithm for coplanar points as presented in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] +class PointTracker +{ +public: + PointTracker(); + + // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) + // f : (focal length)/(sensor width) + // dt : time since last call + bool track(const std::vector& points, float f, float dt); + std::auto_ptr point_model; + + bool dynamic_pose_resolution; + float dt_reset; + + FrameTrafo get_pose() const { return X_CM; } + void reset(); + +protected: + inline cv::Vec2f project(const cv::Vec3f& v_M, float f) + { + cv::Vec3f v_C = X_CM * v_M; + return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); + } + + bool find_correspondences(const std::vector& points, float f); + + cv::Vec2f p[PointModel::N_POINTS]; // the points in model order + cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions + + void predict(float dt); + void update_velocities(float dt); + void reset_velocities(); + + + int POSIT(float f); // The POSIT algorithm, returns the number of iterations + + bool init_phase; + float dt_valid; // time since last valid tracking result + cv::Vec3f v_t; // velocities + cv::Vec3f v_r; + FrameTrafo X_CM; // trafo from model to camera + FrameTrafo X_CM_old; +}; + +#endif //POINTTRACKER_H diff --git a/ftnoir_tracker_pt/resource.h b/ftnoir_tracker_pt/resource.h index c14e94ad..4ca527a3 100644 --- a/ftnoir_tracker_pt/resource.h +++ b/ftnoir_tracker_pt/resource.h @@ -1,14 +1,14 @@ -//{{NO_DEPENDENCIES}} -// Microsoft Visual C++ generated include file. -// Used by FTNoIR_Tracker_PT.rc - -// Next default values for new objects -// -#ifdef APSTUDIO_INVOKED -#ifndef APSTUDIO_READONLY_SYMBOLS -#define _APS_NEXT_RESOURCE_VALUE 101 -#define _APS_NEXT_COMMAND_VALUE 40001 -#define _APS_NEXT_CONTROL_VALUE 1001 -#define _APS_NEXT_SYMED_VALUE 101 -#endif -#endif +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by FTNoIR_Tracker_PT.rc + +// Next default values for new objects +// +#ifdef APSTUDIO_INVOKED +#ifndef APSTUDIO_READONLY_SYMBOLS +#define _APS_NEXT_RESOURCE_VALUE 101 +#define _APS_NEXT_COMMAND_VALUE 40001 +#define _APS_NEXT_CONTROL_VALUE 1001 +#define _APS_NEXT_SYMED_VALUE 101 +#endif +#endif diff --git a/ftnoir_tracker_pt/timer.cpp b/ftnoir_tracker_pt/timer.cpp index 9e6ca8b8..07379853 100644 --- a/ftnoir_tracker_pt/timer.cpp +++ b/ftnoir_tracker_pt/timer.cpp @@ -1,65 +1,65 @@ -/* 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. - */ - -#include "timer.h" - -#include - -// ---------------------------------------------------------------------------- -Timer::Timer() -: startTime(0), endTime(0), running(false) -{ -#ifdef WIN32 - QueryPerformanceFrequency(&frequency); - startCount.QuadPart = 0; - endCount.QuadPart = 0; -#else - startCount.tv_sec = startCount.tv_usec = 0; - endCount.tv_sec = endCount.tv_usec = 0; -#endif -} - - -void Timer::start() -{ -#ifdef WIN32 - QueryPerformanceCounter(&startCount); -#else - gettimeofday(&startCount, NULL); -#endif - running = true; -} - - -void Timer::stop() -{ -#ifdef WIN32 - QueryPerformanceCounter(&endCount); -#else - gettimeofday(&endCount, NULL); -#endif - running = false; -} - - -double Timer::elapsed() -{ -#ifdef WIN32 - if (running) - QueryPerformanceCounter(&endCount); - - startTime = startCount.QuadPart * (1e3 / frequency.QuadPart); - endTime = endCount.QuadPart * (1e3 / frequency.QuadPart); -#else - (void) gettimeofday(&endCount, NULL); - - startTime = (startCount.tv_sec * 1e3) + startCount.tv_usec; - endTime = (endCount.tv_sec * 1e3) + endCount.tv_usec; -#endif - - return endTime - startTime; +/* 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. + */ + +#include "timer.h" + +#include + +// ---------------------------------------------------------------------------- +Timer::Timer() +: startTime(0), endTime(0), running(false) +{ +#ifdef WIN32 + QueryPerformanceFrequency(&frequency); + startCount.QuadPart = 0; + endCount.QuadPart = 0; +#else + startCount.tv_sec = startCount.tv_usec = 0; + endCount.tv_sec = endCount.tv_usec = 0; +#endif +} + + +void Timer::start() +{ +#ifdef WIN32 + QueryPerformanceCounter(&startCount); +#else + gettimeofday(&startCount, NULL); +#endif + running = true; +} + + +void Timer::stop() +{ +#ifdef WIN32 + QueryPerformanceCounter(&endCount); +#else + gettimeofday(&endCount, NULL); +#endif + running = false; +} + + +double Timer::elapsed() +{ +#ifdef WIN32 + if (running) + QueryPerformanceCounter(&endCount); + + startTime = startCount.QuadPart * (1e3 / frequency.QuadPart); + endTime = endCount.QuadPart * (1e3 / frequency.QuadPart); +#else + (void) gettimeofday(&endCount, NULL); + + startTime = (startCount.tv_sec * 1e3) + startCount.tv_usec; + endTime = (endCount.tv_sec * 1e3) + endCount.tv_usec; +#endif + + return endTime - startTime; } \ No newline at end of file diff --git a/ftnoir_tracker_pt/timer.h b/ftnoir_tracker_pt/timer.h index 2aaf725a..1c2c3559 100644 --- a/ftnoir_tracker_pt/timer.h +++ b/ftnoir_tracker_pt/timer.h @@ -1,44 +1,44 @@ -/* 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. - */ - -#ifndef PT_TIMER_H -#define PT_TIMER_H - -#ifdef WIN32 // Windows system specific -#include -#else // Unix based system specific -#include -#endif - -// ---------------------------------------------------------------------------- -// high resolution timer based on http://www.songho.ca/misc/timer/timer.html -class Timer -{ -public: - Timer(); - - void start(); - void stop(); - void restart() { start(); } // for Qt compatibility - double elapsed(); // get elapsed time in ms - -protected: - double startTime; // starting time in ms - double endTime; // ending time in ms - bool running; - -#ifdef WIN32 - LARGE_INTEGER frequency; // ticks per second - LARGE_INTEGER startCount; - LARGE_INTEGER endCount; -#else - timeval startCount; - timeval endCount; -#endif -}; - +/* 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. + */ + +#ifndef PT_TIMER_H +#define PT_TIMER_H + +#ifdef WIN32 // Windows system specific +#include +#else // Unix based system specific +#include +#endif + +// ---------------------------------------------------------------------------- +// high resolution timer based on http://www.songho.ca/misc/timer/timer.html +class Timer +{ +public: + Timer(); + + void start(); + void stop(); + void restart() { start(); } // for Qt compatibility + double elapsed(); // get elapsed time in ms + +protected: + double startTime; // starting time in ms + double endTime; // ending time in ms + bool running; + +#ifdef WIN32 + LARGE_INTEGER frequency; // ticks per second + LARGE_INTEGER startCount; + LARGE_INTEGER endCount; +#else + timeval startCount; + timeval endCount; +#endif +}; + #endif //PT_TIMER_H \ No newline at end of file diff --git a/ftnoir_tracker_pt/trans_calib.cpp b/ftnoir_tracker_pt/trans_calib.cpp index 9b75a1b6..729a0b7f 100644 --- a/ftnoir_tracker_pt/trans_calib.cpp +++ b/ftnoir_tracker_pt/trans_calib.cpp @@ -1,44 +1,44 @@ -/* 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. - */ - -#include "trans_calib.h" - -using namespace cv; - -//----------------------------------------------------------------------------- -TranslationCalibrator::TranslationCalibrator() -{ - reset(); -} - -void TranslationCalibrator::reset() -{ - P = Matx66f::zeros(); - y = Vec6f(0,0,0, 0,0,0); -} - -void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) -{ - Matx H_k_T = Matx::zeros(); - for (int i=0; i<3; ++i) { - for (int j=0; j<3; ++j) { - H_k_T(i,j) = R_CM_k(j,i); - } - } - for (int i=0; i<3; ++i) - { - H_k_T(3+i,i) = 1.0; - } - P += H_k_T * H_k_T.t(); - y += H_k_T * t_CM_k; -} - -Vec3f TranslationCalibrator::get_estimate() -{ - Vec6f x = P.inv() * y; - return Vec3f(-x[0], -x[1], -x[2]); +/* 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. + */ + +#include "trans_calib.h" + +using namespace cv; + +//----------------------------------------------------------------------------- +TranslationCalibrator::TranslationCalibrator() +{ + reset(); +} + +void TranslationCalibrator::reset() +{ + P = Matx66f::zeros(); + y = Vec6f(0,0,0, 0,0,0); +} + +void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) +{ + Matx H_k_T = Matx::zeros(); + for (int i=0; i<3; ++i) { + for (int j=0; j<3; ++j) { + H_k_T(i,j) = R_CM_k(j,i); + } + } + for (int i=0; i<3; ++i) + { + H_k_T(3+i,i) = 1.0; + } + P += H_k_T * H_k_T.t(); + y += H_k_T * t_CM_k; +} + +Vec3f TranslationCalibrator::get_estimate() +{ + Vec6f x = P.inv() * y; + return Vec3f(-x[0], -x[1], -x[2]); } \ No newline at end of file diff --git a/ftnoir_tracker_pt/trans_calib.h b/ftnoir_tracker_pt/trans_calib.h index 4024d011..6288546a 100644 --- a/ftnoir_tracker_pt/trans_calib.h +++ b/ftnoir_tracker_pt/trans_calib.h @@ -1,39 +1,39 @@ -/* 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. - */ - -#ifndef TRANSCALIB_H -#define TRANSCALIB_H - -#include - -//----------------------------------------------------------------------------- -// calibrates the translation from head to model = t_MH -// by recursive least squares / -// kalman filter in information form with identity noise covariance -// measurement equation when head position = t_CH is fixed: -// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k - -class TranslationCalibrator -{ -public: - TranslationCalibrator(); - - // reset the calibration process - void reset(); - - // update the current estimate - void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); - - // get the current estimate for t_MH - cv::Vec3f get_estimate(); - -protected: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) -}; - +/* 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. + */ + +#ifndef TRANSCALIB_H +#define TRANSCALIB_H + +#include + +//----------------------------------------------------------------------------- +// calibrates the translation from head to model = t_MH +// by recursive least squares / +// kalman filter in information form with identity noise covariance +// measurement equation when head position = t_CH is fixed: +// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k + +class TranslationCalibrator +{ +public: + TranslationCalibrator(); + + // reset the calibration process + void reset(); + + // update the current estimate + void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + + // get the current estimate for t_MH + cv::Vec3f get_estimate(); + +protected: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) +}; + #endif //TRANSCALIB_H \ No newline at end of file diff --git a/ftnoir_tracker_pt/video_widget.cpp b/ftnoir_tracker_pt/video_widget.cpp index a13542ba..2b554aba 100644 --- a/ftnoir_tracker_pt/video_widget.cpp +++ b/ftnoir_tracker_pt/video_widget.cpp @@ -1,52 +1,52 @@ -/* 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. - */ - -#include "video_widget.h" - -#include - -using namespace cv; -using namespace std; - -void VideoWidget::update_image(Mat frame, std::auto_ptr< vector >) -{ - QMutexLocker foo(&mtx); - - if (frame.channels() != 3 && frame.channels() != 1) - return; - - int width = frame.cols, height = frame.rows; - unsigned char* src = frame.data; - - QImage qframe(width, height, QImage::Format_RGB888); - if (frame.channels() == 3) - { - uchar* data = qframe.bits(); - const int pitch = qframe.bytesPerLine(); - for (int y = 0; y < height; y++) - for (int x = 0; x < width; x++) - { - const int pos = 3 * (y*width + x); - data[y * pitch + x * 3 + 0] = src[pos + 2]; - data[y * pitch + x * 3 + 1] = src[pos + 1]; - data[y * pitch + x * 3 + 2] = src[pos + 0]; - } - } else { - uchar* data = qframe.bits(); - const int pitch = qframe.bytesPerLine(); - for (int y = 0; y < height; y++) - for (int x = 0; x < width; x++) - { - const int pos = (y*width + x); - data[y * pitch + x * 3 + 0] = src[pos]; - data[y * pitch + x * 3 + 1] = src[pos]; - data[y * pitch + x * 3 + 2] = src[pos]; - } - } - qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation); - pixmap = QPixmap::fromImage(qframe); -} +/* 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. + */ + +#include "video_widget.h" + +#include + +using namespace cv; +using namespace std; + +void VideoWidget::update_image(Mat frame, std::auto_ptr< vector >) +{ + QMutexLocker foo(&mtx); + + if (frame.channels() != 3 && frame.channels() != 1) + return; + + int width = frame.cols, height = frame.rows; + unsigned char* src = frame.data; + + QImage qframe(width, height, QImage::Format_RGB888); + if (frame.channels() == 3) + { + uchar* data = qframe.bits(); + const int pitch = qframe.bytesPerLine(); + for (int y = 0; y < height; y++) + for (int x = 0; x < width; x++) + { + const int pos = 3 * (y*width + x); + data[y * pitch + x * 3 + 0] = src[pos + 2]; + data[y * pitch + x * 3 + 1] = src[pos + 1]; + data[y * pitch + x * 3 + 2] = src[pos + 0]; + } + } else { + uchar* data = qframe.bits(); + const int pitch = qframe.bytesPerLine(); + for (int y = 0; y < height; y++) + for (int x = 0; x < width; x++) + { + const int pos = (y*width + x); + data[y * pitch + x * 3 + 0] = src[pos]; + data[y * pitch + x * 3 + 1] = src[pos]; + data[y * pitch + x * 3 + 2] = src[pos]; + } + } + qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation); + pixmap = QPixmap::fromImage(qframe); +} diff --git a/ftnoir_tracker_pt/video_widget.h b/ftnoir_tracker_pt/video_widget.h index 66959fde..3051919b 100644 --- a/ftnoir_tracker_pt/video_widget.h +++ b/ftnoir_tracker_pt/video_widget.h @@ -1,42 +1,42 @@ -/* 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. - */ - -#ifndef VIDEOWIDGET_H -#define VIDEOWIDGET_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// ---------------------------------------------------------------------------- -class VideoWidget : public QWidget -{ - Q_OBJECT - -public: - VideoWidget(QWidget *parent) : QWidget(parent), mtx() { - } - void update_image(cv::Mat frame, std::auto_ptr< std::vector >); -protected slots: - void paintEvent( QPaintEvent* e ) { - QMutexLocker foo(&mtx); - QPainter painter(this); - painter.drawPixmap(e->rect(), pixmap, e->rect()); - } - -private: - QMutex mtx; - QPixmap pixmap; -}; - -#endif // VIDEOWIDGET_H +/* 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. + */ + +#ifndef VIDEOWIDGET_H +#define VIDEOWIDGET_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ---------------------------------------------------------------------------- +class VideoWidget : public QWidget +{ + Q_OBJECT + +public: + VideoWidget(QWidget *parent) : QWidget(parent), mtx() { + } + void update_image(cv::Mat frame, std::auto_ptr< std::vector >); +protected slots: + void paintEvent( QPaintEvent* e ) { + QMutexLocker foo(&mtx); + QPainter painter(this); + painter.drawPixmap(e->rect(), pixmap, e->rect()); + } + +private: + QMutex mtx; + QPixmap pixmap; +}; + +#endif // VIDEOWIDGET_H -- cgit v1.2.3