diff options
Diffstat (limited to 'ftnoir_tracker_pt')
21 files changed, 2307 insertions, 2307 deletions
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 <QDebug>
-
-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 <QDebug> + +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 <opencv2/opencv.hpp>
-#include <opencv/highgui.h>
-//#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 <opencv2/opencv.hpp> +#include <opencv/highgui.h> +//#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 <QHBoxLayout>
-#include <cmath>
-#include <QDebug>
-#include <QFile>
-#include <QCoreApplication>
-#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<cv::Vec2f>& 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: "<<dt;
- if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps;
- log_stream<<"\n";
-#endif
- refreshVideo();
- }
- msleep(sleep_time);
- }
-
- qDebug()<<"Tracker:: Thread stopping";
-}
-
-void Tracker::apply(const TrackerSettings& settings)
-{
- qDebug()<<"Tracker:: Applying settings";
- QMutexLocker lock(&mutex);
- camera.set_index(settings.cam_index);
- camera.set_res(settings.cam_res_x, settings.cam_res_y);
- camera.set_fps(settings.cam_fps);
- camera.set_f(settings.cam_f);
- point_extractor.threshold_val = settings.threshold;
- point_extractor.min_size = settings.min_point_size;
- point_extractor.max_size = settings.max_point_size;
- point_tracker.point_model = std::auto_ptr<PointModel>(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<Vec2f> > 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<Vec2f> >(new vector<Vec2f>(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 <QHBoxLayout> +#include <cmath> +#include <QDebug> +#include <QFile> +#include <QCoreApplication> +#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<cv::Vec2f>& 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: "<<dt; + if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps; + log_stream<<"\n"; +#endif + refreshVideo(); + } + msleep(sleep_time); + } + + qDebug()<<"Tracker:: Thread stopping"; +} + +void Tracker::apply(const TrackerSettings& settings) +{ + qDebug()<<"Tracker:: Applying settings"; + QMutexLocker lock(&mutex); + camera.set_index(settings.cam_index); + camera.set_res(settings.cam_res_x, settings.cam_res_y); + camera.set_fps(settings.cam_fps); + camera.set_f(settings.cam_f); + point_extractor.threshold_val = settings.threshold; + point_extractor.min_size = settings.min_point_size; + point_extractor.max_size = settings.max_point_size; + point_tracker.point_model = std::auto_ptr<PointModel>(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<Vec2f> > 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<Vec2f> >(new vector<Vec2f>(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 <QThread>
-#include <QMutex>
-#include <QTime>
-#include <opencv2/opencv.hpp>
-#include <QFrame>
-#include <QTimer>
-
-//-----------------------------------------------------------------------------
-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 <QThread> +#include <QMutex> +#include <QTime> +#include <opencv2/opencv.hpp> +#include <QFrame> +#include <QTimer> + +//----------------------------------------------------------------------------- +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 <QMessageBox>
-#include <QDebug>
-#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: "<<t_MH[0]<<t_MH[1]<<t_MH[2];
- ui.tx_spin->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<Tracker*>(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 <QMessageBox> +#include <QDebug> +#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: "<<t_MH[0]<<t_MH[1]<<t_MH[2]; + ui.tx_spin->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<Tracker*>(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 <QTimer>
-
-//-----------------------------------------------------------------------------
-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 <QTimer> + +//----------------------------------------------------------------------------- +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 <QIcon>
-#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 <QIcon> +#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 <QCoreApplication>
-#include <QSettings>
-
-//-----------------------------------------------------------------------------
-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 <QCoreApplication> +#include <QSettings> + +//----------------------------------------------------------------------------- +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 <opencv2/opencv.hpp>
-#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 <opencv2/opencv.hpp> +#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 <QDebug>
-
-using namespace cv;
-using namespace std;
-
-// ----------------------------------------------------------------------------
-const vector<Vec2f>& 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<H; y++)
- {
- for (int x=0; x<W; x++)
- {
- // find connected components with floodfill
- if (frame_bin.at<unsigned char>(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<unsigned char>(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<unsigned char>(i,j) != blob_index-1) continue;
- float val = frame_grey.at<unsigned char>(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 <QDebug> + +using namespace cv; +using namespace std; + +// ---------------------------------------------------------------------------- +const vector<Vec2f>& 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<H; y++) + { + for (int x=0; x<W; x++) + { + // find connected components with floodfill + if (frame_bin.at<unsigned char>(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<unsigned char>(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<unsigned char>(i,j) != blob_index-1) continue; + float val = frame_grey.at<unsigned char>(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 <opencv2/opencv.hpp>
-
-// ----------------------------------------------------------------------------
-// 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<cv::Vec2f>& extract_points(cv::Mat frame, float dt, bool draw_output);
- const std::vector<cv::Vec2f>& get_points() { return points; }
-
- int threshold_val;
- int min_size, max_size;
-
-protected:
- std::vector<cv::Vec2f> 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 <opencv2/opencv.hpp> + +// ---------------------------------------------------------------------------- +// 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<cv::Vec2f>& extract_points(cv::Mat frame, float dt, bool draw_output); + const std::vector<cv::Vec2f>& get_points() { return points; } + + int threshold_val; + int min_size, max_size; + +protected: + std::vector<cv::Vec2f> 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 <vector>
-#include <algorithm>
-#include <cmath>
-
-#include <QDebug>
-
-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<Vec2f> 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<float,int> a, const pair<float,int> b)
-{
- return a.first < b.first;
-}
-
-void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const
-{
- // get sort indices with respect to d scalar product
- vector< pair<float,int> > d_vals;
- for (int i = 0; i<(int)points.size(); ++i)
- d_vals.push_back(pair<float, int>(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<Vec2f>& 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_valid<<" > dt_reset "<<dt_reset;
- reset();
- }
-
- // if there is a pointtracking problem, reset the velocities
- if (!point_model.get() || (int) points.size() != PointModel::N_POINTS)
- {
- //qDebug()<<"Wrong number of points!";
- reset_velocities();
- return false;
- }
-
- X_CM_old = X_CM; // backup old transformation for velocity calculation
-
- if (!init_phase)
- predict(dt_valid);
-
- // if there is a point correspondence problem something has gone wrong, do a reset
- if (!find_correspondences(points, f))
- {
- //qDebug()<<"Error in finding point correspondences!";
- X_CM = X_CM_old; // undo prediction
- reset();
- return false;
- }
-
- (void) POSIT(f);
- //qDebug()<<"Number of POSIT iterations: "<<n_iter;
-
- if (!init_phase)
- update_velocities(dt_valid);
-
- // we have a valid tracking result, leave init phase and reset time since valid result
- init_phase = false;
- dt_valid = 0;
- return true;
-}
-
-void PointTracker::predict(float dt)
-{
- // predict with constant velocity
- Matx33f R;
- Rodrigues(dt*v_r, R);
- X_CM.R = R*X_CM.R;
- X_CM.t += dt * v_t;
-}
-
-void PointTracker::update_velocities(float dt)
-{
- // update velocities
- Rodrigues(X_CM.R*X_CM_old.R.t(), v_r);
- v_r /= dt;
- v_t = (X_CM.t - X_CM_old.t)/dt;
-}
-
-bool PointTracker::find_correspondences(const vector<Vec2f>& 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; i<PointModel::N_POINTS; ++i)
- {
- p[point_model->d_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; i<PointModel::N_POINTS; ++i)
- point_taken[i] = false;
-
- float min_sdist = 0;
- int min_idx = 0;
-
- for (int i=0; i<PointModel::N_POINTS; ++i)
- {
- // find closest point to projected model point i
- for (int j=0; j<PointModel::N_POINTS; ++j)
- {
- Vec2f d = p_exp[i]-points[j];
- float sdist = d.dot(d);
- if (sdist < min_sdist || j==0)
- {
- min_idx = j;
- min_sdist = sdist;
- }
- }
- // if one point is closest to more than one model point, abort
- if (point_taken[min_idx]) return false;
- point_taken[min_idx] = true;
- p[i] = points[min_idx];
- }
- }
- return true;
-}
-
-
-
-int PointTracker::POSIT(float f)
-{
- // POSIT algorithm for coplanar points as presented in
- // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
- // we use the same notation as in the paper here
-
- // The expected rotation used for resolving the ambiguity in POSIT:
- // In every iteration step the rotation closer to R_expected is taken
- Matx33f R_expected;
- if (init_phase)
- R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation
- else
- R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation
-
- // initial pose = last (predicted) pose
- Vec3f k;
- get_row(X_CM.R, 2, k);
- float Z0 = X_CM.t[2];
-
- float old_epsilon_1 = 0;
- float old_epsilon_2 = 0;
- float epsilon_1 = 1;
- float epsilon_2 = 1;
-
- Vec3f I0, J0;
- Vec2f I0_coeff, J0_coeff;
-
- Vec3f I_1, J_1, I_2, J_2;
- Matx33f R_1, R_2;
- Matx33f* R_current;
-
- const int MAX_ITER = 100;
- const float EPS_THRESHOLD = 1e-4;
-
- int i=1;
- for (; i<MAX_ITER; ++i)
- {
- epsilon_1 = k.dot(point_model->M01)/Z0;
- epsilon_2 = k.dot(point_model->M02)/Z0;
-
- // vector of scalar products <I0, M0i> and <J0, M0i>
- 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: "<<i;
- //qDebug()<<"t: "<<X_CM.t[0]<<' '<<X_CM.t[1]<<' '<<X_CM.t[2];
- //Vec3f r;
- //
- //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';
-}
+/* 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 <vector> +#include <algorithm> +#include <cmath> + +#include <QDebug> + +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<Vec2f> 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<float,int> a, const pair<float,int> b) +{ + return a.first < b.first; +} + +void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const +{ + // get sort indices with respect to d scalar product + vector< pair<float,int> > d_vals; + for (int i = 0; i<(int)points.size(); ++i) + d_vals.push_back(pair<float, int>(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<Vec2f>& 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_valid<<" > dt_reset "<<dt_reset; + reset(); + } + + // if there is a pointtracking problem, reset the velocities + if (!point_model.get() || (int) points.size() != PointModel::N_POINTS) + { + //qDebug()<<"Wrong number of points!"; + reset_velocities(); + return false; + } + + X_CM_old = X_CM; // backup old transformation for velocity calculation + + if (!init_phase) + predict(dt_valid); + + // if there is a point correspondence problem something has gone wrong, do a reset + if (!find_correspondences(points, f)) + { + //qDebug()<<"Error in finding point correspondences!"; + X_CM = X_CM_old; // undo prediction + reset(); + return false; + } + + (void) POSIT(f); + //qDebug()<<"Number of POSIT iterations: "<<n_iter; + + if (!init_phase) + update_velocities(dt_valid); + + // we have a valid tracking result, leave init phase and reset time since valid result + init_phase = false; + dt_valid = 0; + return true; +} + +void PointTracker::predict(float dt) +{ + // predict with constant velocity + Matx33f R; + Rodrigues(dt*v_r, R); + X_CM.R = R*X_CM.R; + X_CM.t += dt * v_t; +} + +void PointTracker::update_velocities(float dt) +{ + // update velocities + Rodrigues(X_CM.R*X_CM_old.R.t(), v_r); + v_r /= dt; + v_t = (X_CM.t - X_CM_old.t)/dt; +} + +bool PointTracker::find_correspondences(const vector<Vec2f>& 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; i<PointModel::N_POINTS; ++i) + { + p[point_model->d_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; i<PointModel::N_POINTS; ++i) + point_taken[i] = false; + + float min_sdist = 0; + int min_idx = 0; + + for (int i=0; i<PointModel::N_POINTS; ++i) + { + // find closest point to projected model point i + for (int j=0; j<PointModel::N_POINTS; ++j) + { + Vec2f d = p_exp[i]-points[j]; + float sdist = d.dot(d); + if (sdist < min_sdist || j==0) + { + min_idx = j; + min_sdist = sdist; + } + } + // if one point is closest to more than one model point, abort + if (point_taken[min_idx]) return false; + point_taken[min_idx] = true; + p[i] = points[min_idx]; + } + } + return true; +} + + + +int PointTracker::POSIT(float f) +{ + // POSIT algorithm for coplanar points as presented in + // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] + // we use the same notation as in the paper here + + // The expected rotation used for resolving the ambiguity in POSIT: + // In every iteration step the rotation closer to R_expected is taken + Matx33f R_expected; + if (init_phase) + R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation + else + R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation + + // initial pose = last (predicted) pose + Vec3f k; + get_row(X_CM.R, 2, k); + float Z0 = X_CM.t[2]; + + float old_epsilon_1 = 0; + float old_epsilon_2 = 0; + float epsilon_1 = 1; + float epsilon_2 = 1; + + Vec3f I0, J0; + Vec2f I0_coeff, J0_coeff; + + Vec3f I_1, J_1, I_2, J_2; + Matx33f R_1, R_2; + Matx33f* R_current; + + const int MAX_ITER = 100; + const float EPS_THRESHOLD = 1e-4; + + int i=1; + for (; i<MAX_ITER; ++i) + { + epsilon_1 = k.dot(point_model->M01)/Z0; + epsilon_2 = k.dot(point_model->M02)/Z0; + + // vector of scalar products <I0, M0i> and <J0, M0i> + 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: "<<i; + //qDebug()<<"t: "<<X_CM.t[0]<<' '<<X_CM.t[1]<<' '<<X_CM.t[2]; + //Vec3f r; + // + //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n'; +} diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h index 7eee580d..8967f806 100644 --- a/ftnoir_tracker_pt/point_tracker.h +++ b/ftnoir_tracker_pt/point_tracker.h @@ -1,114 +1,114 @@ -/* 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 <memory>
-#include <opencv2/opencv.hpp>
-#include <list>
-
-// ----------------------------------------------------------------------------
-// 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<cv::Vec2f>& 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<cv::Vec2f>& points, float f, float dt);
- std::auto_ptr<PointModel> 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<cv::Vec2f>& 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 <memory> +#include <opencv2/opencv.hpp> +#include <list> + +// ---------------------------------------------------------------------------- +// 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<cv::Vec2f>& 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<cv::Vec2f>& points, float f, float dt); + std::auto_ptr<PointModel> 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<cv::Vec2f>& 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 <stdlib.h>
-
-// ----------------------------------------------------------------------------
-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 <stdlib.h> + +// ---------------------------------------------------------------------------- +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 <windows.h>
-#else // Unix based system specific
-#include <sys/time.h>
-#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 <windows.h> +#else // Unix based system specific +#include <sys/time.h> +#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<float, 6,3> H_k_T = Matx<float, 6,3>::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<float, 6,3> H_k_T = Matx<float, 6,3>::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 <opencv2/opencv.hpp>
-
-//-----------------------------------------------------------------------------
-// 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 <opencv2/opencv.hpp> + +//----------------------------------------------------------------------------- +// 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 <QDebug>
-
-using namespace cv;
-using namespace std;
-
-void VideoWidget::update_image(Mat frame, std::auto_ptr< vector<Vec2f> >)
-{
- 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 <QDebug> + +using namespace cv; +using namespace std; + +void VideoWidget::update_image(Mat frame, std::auto_ptr< vector<Vec2f> >) +{ + 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 <QTime>
-#include <opencv2/opencv.hpp>
-#include <memory>
-#include <QWidget>
-#include <QMutex>
-#include <QMutexLocker>
-#include <QLabel>
-#include <QPainter>
-#include <QPaintEvent>
-
-// ----------------------------------------------------------------------------
-class VideoWidget : public QWidget
-{
- Q_OBJECT
-
-public:
- VideoWidget(QWidget *parent) : QWidget(parent), mtx() {
- }
- void update_image(cv::Mat frame, std::auto_ptr< std::vector<cv::Vec2f> >);
-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 <QTime> +#include <opencv2/opencv.hpp> +#include <memory> +#include <QWidget> +#include <QMutex> +#include <QMutexLocker> +#include <QLabel> +#include <QPainter> +#include <QPaintEvent> + +// ---------------------------------------------------------------------------- +class VideoWidget : public QWidget +{ + Q_OBJECT + +public: + VideoWidget(QWidget *parent) : QWidget(parent), mtx() { + } + void update_image(cv::Mat frame, std::auto_ptr< std::vector<cv::Vec2f> >); +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 |