From 4c95700fc6848d46a33b565df7eb81ada2b32987 Mon Sep 17 00:00:00 2001 From: Patrick Ruoff Date: Mon, 31 Dec 2012 12:33:18 +0000 Subject: Updated PointTracker git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@200 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb --- FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui | 1165 +++++++++++++++------- FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj | 66 +- FTNoIR_Tracker_PT/Resources/cap_front.png | Bin 0 -> 1164 bytes FTNoIR_Tracker_PT/Resources/cap_side.png | Bin 0 -> 1733 bytes FTNoIR_Tracker_PT/Resources/clip_front.png | Bin 0 -> 571 bytes FTNoIR_Tracker_PT/Resources/clip_side.png | Bin 0 -> 2677 bytes FTNoIR_Tracker_PT/camera.cpp | 224 ++++- FTNoIR_Tracker_PT/camera.h | 85 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp | 48 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.h | 5 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.qrc | 12 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp | 134 ++- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h | 31 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dll.cpp | 2 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp | 59 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h | 35 +- FTNoIR_Tracker_PT/point_extractor.cpp | 34 +- FTNoIR_Tracker_PT/point_tracker.cpp | 165 ++- FTNoIR_Tracker_PT/point_tracker.h | 25 +- FTNoIR_Tracker_PT/timer.cpp | 66 ++ FTNoIR_Tracker_PT/timer.h | 44 + FTNoIR_Tracker_PT/videoInput/videoInput.h | 385 +++++++ FTNoIR_Tracker_PT/videoInput/videoInput_vc8.lib | Bin 0 -> 2145206 bytes FTNoIR_Tracker_PT/videoInput/videoInput_vc9.lib | Bin 0 -> 2119546 bytes 24 files changed, 1993 insertions(+), 592 deletions(-) create mode 100644 FTNoIR_Tracker_PT/Resources/cap_front.png create mode 100644 FTNoIR_Tracker_PT/Resources/cap_side.png create mode 100644 FTNoIR_Tracker_PT/Resources/clip_front.png create mode 100644 FTNoIR_Tracker_PT/Resources/clip_side.png create mode 100644 FTNoIR_Tracker_PT/timer.cpp create mode 100644 FTNoIR_Tracker_PT/timer.h create mode 100644 FTNoIR_Tracker_PT/videoInput/videoInput.h create mode 100644 FTNoIR_Tracker_PT/videoInput/videoInput_vc8.lib create mode 100644 FTNoIR_Tracker_PT/videoInput/videoInput_vc9.lib diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui index 6ba6a1db..9c70b397 100644 --- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui +++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui @@ -9,8 +9,8 @@ 0 0 - 395 - 511 + 401 + 497 @@ -50,6 +50,9 @@ 0 + + + 0 @@ -66,30 +69,6 @@ - - - - - - Show VideoWidget - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - @@ -109,6 +88,9 @@ + + Time the tracker thread sleeps after each processed frame + @@ -140,6 +122,9 @@ + + Time until automatic reset of tracker's internal state when no valid tracking result is found + 9999 @@ -148,7 +133,7 @@ - s + ms @@ -156,34 +141,8 @@ - - - - - - - - - Camera - - - - - - - - Index - - - camindex_spin - - - - - - - + Qt::Horizontal @@ -196,62 +155,75 @@ - - - (Focal length)/(Sensor width) - - - f_dspin - - - - - - - 0.100000000000000 + + + QFormLayout::AllNonFixedFieldsGrow - + + + + Whether to update the content of the VideoWidget + + + Show VideoWidget + + + + + + + + + + + Camera + + - - - - - Resolution - - - - - - - 2000 - - - 10 - - - - - - - x - - - + - - - 2000 - - - 10 - - + + + + + Index + + + camindex_spin + + + + + + + Capture device index + + + + + + + FPS + + + fps_spin + + + + + + + Desired capture framerate + + + + - + Qt::Horizontal @@ -264,17 +236,92 @@ - - - FPS - - - fps_spin - - - - - + + + + + + + Resolution + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + Desired capture width + + + 2000 + + + 10 + + + + + + + x + + + + + + + Desired capture height + + + 2000 + + + 10 + + + + + + + + + + + (Focal length)/(Sensor width) + + + f_dspin + + + + + + + The camera's focal length devided by its sensor width + + + 2 + + + 0.100000000000000 + + + + + + @@ -301,6 +348,9 @@ + + Intensity threshold for point extraction + 255 @@ -327,7 +377,11 @@ - + + + Minimum point diameter + + @@ -360,7 +414,11 @@ - + + + Maximum point diameter + + @@ -382,7 +440,7 @@ 20 - 40 + 20 @@ -447,28 +505,33 @@ - - + false + + Center the tracker + - Reset + Center - + false + + Reset the tracker's internal state + - Center + Reset @@ -476,230 +539,628 @@ - + Model - + - - - Model Dimensions + + + QTabWidget::Rounded - - - - - <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p></body></html> - - - - - - - - - - - z: - - - - - - - -999 - - - 999 - - - - - - - y: - - - - - - - -999 - - - 999 - - - - - - - mm - - - - - - - M1: - - - - - - - -999 + + 0 + + + false + + + false + + + false + + + + Clip + + + + + + Model Dimensions (mm) + + + + + + + 0 + 0 + - - 999 + + + 150 + 170 + + + + + 30 + 30 + 71 + 111 + + + + + + + :/Resources/clip_side.png + + + + + + 100 + 50 + 46 + 22 + + + + 999 + + + + + + 60 + 10 + 46 + 22 + + + + 999 + + + + + + 100 + 90 + 46 + 22 + + + + 999 + + + + + + 10 + 10 + 46 + 13 + + + + Side + + + + + + 40 + 140 + 46 + 22 + + + + 999 + + + + + + 70 + 70 + 16 + 16 + + + + R + + - - - - mm + + + + + 0 + 0 + - - - - - - mm - - - - - - - x: + + + 100 + 140 + + + + + 10 + 10 + 46 + 13 + + + + Front + + + + + + 40 + 30 + 21 + 111 + + + + + + + :/Resources/clip_front.png + + + + + + 60 + 70 + 16 + 16 + + + + R + + - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - M2: + + + + + + + Cap + + + + + + Model Dimensions (mm) + + + + + + + 140 + 130 + + + + + 20 + 50 + 111 + 81 + + + + + + + :/Resources/cap_side.png + + + + + + 30 + 80 + 46 + 22 + + + + 999 + + + + + + 130 + 50 + 16 + 16 + + + + R + + + + + + 10 + 10 + 46 + 13 + + + + Side + + + + + + 50 + 40 + 46 + 22 + + + + 999 + + - - - - + + + + + 0 + 0 + - - -999 - - - 999 + + + 100 + 130 + + + + + 10 + 10 + 46 + 13 + + + + Front + + + + + + 30 + 50 + 16 + 16 + + + + R + + + + + + 10 + 50 + 81 + 81 + + + + + + + :/Resources/cap_front.png + + + + + + 50 + 30 + 46 + 22 + + + + 999 + + - - + + + + + + + + Custom + + + + + + Model Dimensions (mm) + + + + - mm + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p></body></html> - - - - -999 + + + + Qt::Vertical - - 999 + + + 20 + 20 + - + - - - - mm - - - - - - - x: - - - - - - - mm - - - - - - - z: - - + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + + + -999 + + + 999 + + + + + + + y: + + + + + + + -999 + + + 999 + + + + + + + z: + + + + + + + M1: + + + + + + + -999 + + + 999 + + + + + + + x: + + + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + + + -999 + + + 999 + + + + + + + x: + + + + + + + z: + + + + + + + -999 + + + 999 + + + + + + + y: + + + + + + + M2: + + + + + + + + + + -999 + + + 999 + + + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + - - - - -999 + + + + Qt::Vertical - - 999 + + + 20 + 20 + - - - - - - y: - - + - - - - + + + + - + - Model Translation + Model Position (mm) - + - + <html><head/><body><p>Translation from head center to model reference point<br/> in default pose</p></body></html> - + + + + + Qt::Horizontal + + + + 20 + 20 + + + + - + @@ -714,40 +1175,26 @@ - + x: - + y: - - - - mm - - - - + z: - - - - mm - - - @@ -758,13 +1205,6 @@ - - - - mm - - - @@ -776,7 +1216,7 @@ - + T: @@ -785,13 +1225,13 @@ - + Qt::Horizontal - 40 + 20 20 @@ -810,55 +1250,24 @@ + + + + Qt::Horizontal + + + + 20 + 20 + + + + - - - - Camera frame = Model frame in default pose: - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - <html><head/><body><p>z axis: Out of camera to you<br/>y axis: Upwards<br/>x axis: To the right</p></body></html> - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - @@ -875,7 +1284,7 @@ - <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 0.2</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.0 beta</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> true @@ -937,19 +1346,27 @@ tabWidget ok_button cancel_button - videowidget_check sleep_spin reset_spin + videowidget_check camindex_spin - f_dspin res_x_spin res_y_spin fps_spin + f_dspin threshold_slider mindiam_spin maxdiam_spin - reset_button center_button + reset_button + model_tabs + clip_tlength_spin + clip_theight_spin + clip_bheight_spin + clip_blength_spin + cap_length_spin + cap_height_spin + cap_width_spin m1x_spin m1y_spin m1z_spin diff --git a/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj b/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj index 51d2a38a..3c895e51 100644 --- a/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj +++ b/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj @@ -4,6 +4,7 @@ Version="9,00" Name="FTNoIR_Tracker_PT" ProjectGUID="{7A2A2560-9253-4CC8-A9D5-4B9D9C224D9D}" + RootNamespace="FTNoIR_Tracker_PT" Keyword="Qt4VSv1.0" TargetFrameworkVersion="0" > @@ -39,8 +40,6 @@ @@ -128,9 +127,10 @@ /> @@ -193,6 +193,10 @@ RelativePath=".\point_tracker.cpp" > + + @@ -287,6 +291,10 @@ RelativePath=".\resource.h" > + + @@ -382,46 +390,6 @@ /> - - - - - - - - - - - - - - - - using namespace cv; // ---------------------------------------------------------------------------- -Camera::Camera() - : dt_valid(0), dt_mean(0), cap(NULL), active_index(-1) -{} - -Camera::~Camera() -{ - if (cap) cvReleaseCapture(&cap); -} - void Camera::set_index(int index) { - if (index == active_index) return; - if (cap) cvReleaseCapture(&cap); + if (desired_index != index) + { + desired_index = index; + _set_index(); - cap = cvCreateCameraCapture(index); + // reset fps + dt_valid = 0; + dt_mean = 0; + active_index = index; + } +} - // extract camera info - if (cap) +void Camera::set_f(float f) +{ + if (cam_desired.f != f) { - cam_info.res_x = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_WIDTH); - cam_info.res_y = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_HEIGHT); + cam_desired.f = f; + _set_f(); + } +} +void Camera::set_fps(int fps) +{ + if (cam_desired.fps != fps) + { + cam_desired.fps = fps; + _set_fps(); } +} - active_index = index; - dt_mean = 0; // reset fps calculation +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::set_fps(int fps) +bool Camera::get_frame(float dt, cv::Mat* frame) { - return cap && cvSetCaptureProperty(cap, CV_CAP_PROP_FPS, fps); + 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; } -bool Camera::set_res(int x_res, int y_res) +// ---------------------------------------------------------------------------- +/* +void CVCamera::start() { + cap = cvCreateCameraCapture(desired_index); + // extract camera info if (cap) { - if (x_res == cam_info.res_x && y_res == cam_info.res_y) return true; - cvSetCaptureProperty(cap, CV_CAP_PROP_FRAME_WIDTH, x_res); - cvSetCaptureProperty(cap, CV_CAP_PROP_FRAME_HEIGHT, y_res); + active = true; + active_index = desired_index; cam_info.res_x = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_WIDTH); cam_info.res_y = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_HEIGHT); - if (x_res == cam_info.res_x && y_res == cam_info.res_y) return true; } - return false; } -cv::Mat Camera::get_frame(float dt) +void CVCamera::stop() +{ + if (cap) cvReleaseCapture(&cap); + active = false; +} + +bool CVCamera::_get_frame(Mat* frame) { - cv::Mat frame; if (cap && cvGrabFrame(cap) != 0) { // retrieve frame @@ -59,24 +97,130 @@ cv::Mat Camera::get_frame(float dt) if(_img) { if(_img->origin == IPL_ORIGIN_TL) - frame = Mat(_img); + *frame = Mat(_img); else { Mat temp(_img); - flip(temp, frame, 0); - } + flip(temp, *frame, 0); + } + return true; } } + return false; +} - // measure fps of valid frames - const float dt_smoothing_const = 0.9; - dt_valid += dt; - if (!frame.empty()) +void CVCamera::_set_index() +{ + if (active) restart(); +} + +void CVCamera::_set_f() +{ + cam_info.f = cam_desired.f; +} + +void CVCamera::_set_fps() +{ + if (cap) cvSetCaptureProperty(cap, CV_CAP_PROP_FPS, cam_desired.fps); +} + +void CVCamera::_set_res() +{ + if (cap) { - dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid; - cam_info.fps = 1.0 / dt_mean; - dt_valid = 0; + cvSetCaptureProperty(cap, CV_CAP_PROP_FRAME_WIDTH, cam_desired.res_x); + cvSetCaptureProperty(cap, CV_CAP_PROP_FRAME_HEIGHT, cam_desired.res_y); + cam_info.res_x = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_WIDTH); + cam_info.res_y = cvGetCaptureProperty(cap, CV_CAP_PROP_FRAME_HEIGHT); + } +} +*/ + +// ---------------------------------------------------------------------------- +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(); +} - return frame; -} \ No newline at end of file diff --git a/FTNoIR_Tracker_PT/camera.h b/FTNoIR_Tracker_PT/camera.h index f26a7a8f..cd1f0842 100644 --- a/FTNoIR_Tracker_PT/camera.h +++ b/FTNoIR_Tracker_PT/camera.h @@ -9,10 +9,13 @@ #define CAMERA_H #include +#include "videoInput/videoInput.h" // ---------------------------------------------------------------------------- struct CamInfo { + CamInfo() : res_x(0), res_y(0), fps(0), f(1) {} + int res_x; int res_y; int fps; @@ -20,30 +23,94 @@ struct CamInfo }; // ---------------------------------------------------------------------------- +// base class for cameras class Camera { public: - - Camera(); - ~Camera(); + Camera() : dt_valid(0), dt_mean(0), desired_index(0), active_index(-1), active(false) {} + 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) { cam_info.f = f; } - bool set_fps(int fps); - bool set_res(int x_res, int y_res); + 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 - cv::Mat get_frame(float dt); + bool get_frame(float dt, cv::Mat* frame); - // WARNING: returned reference is valid as long as object + // 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; + + bool active; + int desired_index; int active_index; - CvCapture* cap; CamInfo cam_info; + CamInfo cam_desired; float dt_valid; float dt_mean; }; + +// ---------------------------------------------------------------------------- +// 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(); + + CvCapture* cap; +}; +*/ + +// ---------------------------------------------------------------------------- +// videoInput camera +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 //CAMERA_H diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp index aa0e4272..13522863 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp @@ -9,25 +9,30 @@ #include #include #include +#include +#include using namespace std; using namespace cv; using namespace boost; +//#define PT_PERF_LOG //log performance + //----------------------------------------------------------------------------- Tracker::Tracker() : frame_count(0), commands(0), video_widget(NULL) { - qDebug()<<"Tracker Const"; + qDebug()<<"Tracker::Tracker"; TrackerSettings settings; settings.load_ini(); apply(settings); - qDebug()<<"Tracker Starting"; + camera.start(); start(); } Tracker::~Tracker() { + qDebug()<<"Tracker::~Tracker"; set_command(ABORT); wait(); if (video_widget) delete video_widget; @@ -47,8 +52,17 @@ void Tracker::reset_command(Command command) void Tracker::run() { - qDebug()<<"Tracker Thread started"; + 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 { { @@ -58,47 +72,55 @@ void Tracker::run() if (commands & PAUSE) continue; commands = 0; - float dt = time.elapsed() / 1000.0; + dt = time.elapsed() / 1000.0; time.restart(); - frame = camera.get_frame(dt); - if (!frame.empty()) + new_frame = camera.get_frame(dt, &frame); + if (new_frame && !frame.empty()) { const std::vector& points = point_extractor.extract_points(frame, dt, draw_frame); point_tracker.track(points, camera.get_info().f, dt); frame_count++; } +#ifdef PT_PERF_LOG + log_stream<<"dt: "<(new PointModel(settings.M01, settings.M02)); sleep_time = settings.sleep_time; - // TODO: reset time + point_tracker.dt_reset = settings.reset_time / 1000.0; draw_frame = settings.video_widget; t_MH = settings.t_MH; } void Tracker::reset() { - //TODO + 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); @@ -112,7 +134,7 @@ void Tracker::Initialize(QFrame *videoframe) const int VIDEO_FRAME_WIDTH = 252; const int VIDEO_FRAME_HEIGHT = 189; - qDebug("Tracker::Initialize()"); + qDebug("Tracker::Initialize"); // setup video frame videoframe->setAttribute(Qt::WA_NativeWindow); videoframe->show(); diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h index 5461d133..bfc262b3 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h @@ -14,6 +14,7 @@ #include "point_extractor.h" #include "point_tracker.h" #include "video_widget.h" +#include "timer.h" #include #include @@ -57,7 +58,7 @@ protected: void reset_command(Command command); int commands; - Camera camera; + VICamera camera; PointExtractor point_extractor; PointTracker point_tracker; cv::Vec3f t_MH; @@ -67,7 +68,7 @@ protected: long frame_count; VideoWidget* video_widget; - QTime time; + Timer time; }; #endif // FTNOIR_TRACKER_PT_H diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.qrc b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.qrc index d1dab18a..eb1fba2c 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.qrc +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.qrc @@ -1,6 +1,10 @@ - - Resources/icon.ico - Resources/Logo_IR.png - + + Resources/icon.ico + Resources/cap_front.png + Resources/cap_side.png + Resources/clip_front.png + Resources/clip_side.png + Resources/Logo_IR.png + diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp index fca18fee..357cd67f 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp @@ -20,6 +20,7 @@ TrackerDialog::TrackerDialog() ui.setupUi( this ); settings.load_ini(); + dialog_settings.load_ini(); // initialize ui values ui.videowidget_check->setChecked(settings.video_widget); @@ -33,39 +34,53 @@ TrackerDialog::TrackerDialog() ui.threshold_slider->setValue(settings.threshold); ui.mindiam_spin->setValue(settings.min_point_size); ui.maxdiam_spin->setValue(settings.max_point_size); - - ui.m1x_spin->setValue(settings.M01[0]); - ui.m1y_spin->setValue(settings.M01[1]); - ui.m1z_spin->setValue(settings.M01[2]); - ui.m2x_spin->setValue(settings.M02[0]); - ui.m2y_spin->setValue(settings.M02[1]); - ui.m2z_spin->setValue(settings.M02[2]); + 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.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.threshold_slider,SIGNAL(sliderMoved(int)), this,SLOT(set_threshold(int)) ); - 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.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.videowidget_check,SIGNAL(toggled(bool)), this,SLOT(set_video_widget(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.threshold_slider,SIGNAL(sliderMoved(int)), this,SLOT(set_threshold(int)) ); + 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)) ); @@ -84,6 +99,65 @@ 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) @@ -135,6 +209,7 @@ void TrackerDialog::doReset() void TrackerDialog::doOK() { settings.save_ini(); + dialog_settings.save_ini(); close(); } @@ -146,6 +221,7 @@ void TrackerDialog::doCancel() switch (ret) { case QMessageBox::Save: settings.save_ini(); + dialog_settings.save_ini(); close(); break; case QMessageBox::Discard: diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h index e5a27dc5..68baf87a 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h @@ -44,15 +44,23 @@ protected slots: 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_m1x(int val) { settings.M01[0] = val; settings_changed(); } - void set_m1y(int val) { settings.M01[1] = val; settings_changed(); } - void set_m1z(int val) { settings.M01[2] = val; settings_changed(); } - void set_m2x(int val) { settings.M02[0] = val; settings_changed(); } - void set_m2y(int val) { settings.M02[1] = val; settings_changed(); } - void set_m2z(int val) { settings.M02[2] = val; settings_changed(); } - 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_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(); @@ -65,9 +73,14 @@ protected slots: 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; diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dll.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dll.cpp index d41e5db2..9ceaa321 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dll.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dll.cpp @@ -12,7 +12,7 @@ //----------------------------------------------------------------------------- void TrackerDll::getFullName(QString *strToBeFilled) { - *strToBeFilled = "PointTracker 0.2"; + *strToBeFilled = "PointTracker 1.0 beta"; } void TrackerDll::getShortName(QString *strToBeFilled) diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp index d67ced5e..036b0558 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp @@ -13,8 +13,8 @@ void TrackerSettings::load_ini() { qDebug("TrackerSettings::load_ini()"); - QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // 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) @@ -39,7 +39,7 @@ void TrackerSettings::load_ini() t_MH[2] = iniFile.value("tMHz", 0).toFloat(); video_widget = iniFile.value("VideoWidget", true).toBool(); sleep_time = iniFile.value("SleepTime", 10).toInt(); - reset_time = iniFile.value("ResetTime", 3).toInt(); + reset_time = iniFile.value("ResetTime", 1000).toInt(); iniFile.endGroup(); } @@ -47,8 +47,8 @@ void TrackerSettings::load_ini() void TrackerSettings::save_ini() const { qDebug("TrackerSettings::save_ini()"); - QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // 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) @@ -76,4 +76,57 @@ void TrackerSettings::save_ini() const iniFile.setValue("ResetTime", reset_time); iniFile.endGroup(); +} + +//----------------------------------------------------------------------------- +void TrackerDialogSettings::load_ini() +{ + qDebug("TrackerDialogSettings::load_ini()"); + + QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // 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("Abbequerque Inc.", "FaceTrackNoIR"); // 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 8868f418..23496d91 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h @@ -11,9 +11,10 @@ #include #include "point_tracker.h" + //----------------------------------------------------------------------------- struct TrackerSettings -{ +{ // camera int cam_index; float cam_f; @@ -34,11 +35,41 @@ struct TrackerSettings cv::Vec3f t_MH; int sleep_time; // in ms - int reset_time; // in s + int reset_time; // in ms bool video_widget; 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 b26f7068..4aa1a658 100644 --- a/FTNoIR_Tracker_PT/point_extractor.cpp +++ b/FTNoIR_Tracker_PT/point_extractor.cpp @@ -6,7 +6,6 @@ */ #include "point_extractor.h" - #include using namespace cv; @@ -39,36 +38,7 @@ const vector& PointExtractor::extract_points(Mat frame, float dt, bool dr //erode(frame_bw, frame_bw, Mat(), Point(-1,-1), min_size); //destroys information -> bad for subpixel accurarcy // find connected components... - // Method 1: contours - //* - // find contours - vector< vector > contours; - findContours(frame_bw.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); - - // extract points - // TODO: use proximity to old points for classification - float r; - Vec2f c; - Point2f dummy; - points.clear(); - for (vector< vector >::iterator iter = contours.begin(); - iter!= contours.end(); - ++iter) - { - minEnclosingCircle(*iter, dummy, r); - if (r > max_size) continue; - Moments m = moments(*iter); - if (m.m00 < 3.14*min_size*min_size) continue; - // convert to centered camera coordinate system with y axis upwards - c[0] = (m.m10/m.m00 - frame.cols/2)/frame.cols; - c[1] = -(m.m01/m.m00 - frame.rows/2)/frame.cols; - points.push_back(c); - } - //*/ - - // Method 2: floodfill - /* - // extract blobs + // extract blobs with floodfill struct BlobInfo { BlobInfo() : m00(0), m10(0), m01(0) {} @@ -97,6 +67,7 @@ const vector& PointExtractor::extract_points(Mat frame, float dt, bool dr blob_count++; if (blob_count >= 255) break; } + if (blob_count >= 255) break; } // extract points @@ -115,7 +86,6 @@ const vector& PointExtractor::extract_points(Mat frame, float dt, bool dr c[1] = -(m.m01/float(m.m00) - frame.rows/2)/frame.cols; points.push_back(c); } - */ // draw output image if (draw_output) diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index 90093032..03914bb4 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -87,37 +87,159 @@ void PointModel::get_d_order(const std::vector& points, int d_order[] // ---------------------------------------------------------------------------- -PointTracker::PointTracker() +PointTracker::PointTracker() : init_phase(true), dt_valid(0), dt_reset(1), v_t(0,0,0), v_r(0,0,0) { - X_CM.t[2] = 1000; // default position: 1 m away from cam + X_CM.t[2] = 1000; // default position: 1 m away from cam; } +void PointTracker::reset() +{ + // enter init phase and reset velocities + init_phase = true; + dt_valid = 0; + reset_velocities(); +} + +void PointTracker::reset_velocities() +{ + v_t = Vec3f(0,0,0); + v_r = Vec3f(0,0,0); +} + + bool PointTracker::track(const vector& points, float f, float dt) { - if (!point_model) return false; - if (!find_correspondences(points)) return false; - POSIT(f); + dt_valid += dt; + // if there was no valid tracking result for too long, do a reset + if (dt_valid > dt_reset) + { + //qDebug()<<"dt_valid "< dt_reset "<& points) +void PointTracker::predict(float dt) { - if (points.size() != PointModel::N_POINTS) return false; + // predict with constant velocity + Matx33f R; + Rodrigues(dt*v_r, R); + X_CM.R = R*X_CM.R; + X_CM.t += dt * v_t; +} - // sort points - int point_d_order[PointModel::N_POINTS]; - point_model->get_d_order(points, point_d_order); +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; +} - // set correspondences - for (int i=0; id_order[i]] = points[point_d_order[i]]; +bool PointTracker::find_correspondences(const vector& points, float f) +{ + if (init_phase) { + // We do a simple freetrack-like sorting in the init phase... + // sort points + int point_d_order[PointModel::N_POINTS]; + point_model->get_d_order(points, point_d_order); + + // set correspondences + for (int i=0; id_order[i]] = points[point_d_order[i]]; + } + } + else { + // ... otherwise we look at the distance to the projection of the expected model points + // project model points under current pose + p_exp[0] = project(Vec3f(0,0,0), f); + p_exp[1] = project(point_model->M01, f); + p_exp[2] = project(point_model->M02, f); + + // set correspondences by minimum distance to projected model point + bool point_taken[PointModel::N_POINTS]; + for (int i=0; i& points, float f, float dt); boost::shared_ptr point_model; + float dt_reset; + FrameTrafo get_pose() const { return X_CM; } + void reset(); protected: - bool find_correspondences(const std::vector& points); + inline cv::Vec2f project(const cv::Vec3f& v_M, float f) + { + cv::Vec3f v_C = X_CM * v_M; + return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); + } + + bool find_correspondences(const std::vector& points, float f); + cv::Vec2f p[PointModel::N_POINTS]; // the points in model order + cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions + + void predict(float dt); + void update_velocities(float dt); + void reset_velocities(); - void POSIT(float f); + + 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/timer.cpp b/FTNoIR_Tracker_PT/timer.cpp new file mode 100644 index 00000000..363b5b09 --- /dev/null +++ b/FTNoIR_Tracker_PT/timer.cpp @@ -0,0 +1,66 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "timer.h" + +#include + +// ---------------------------------------------------------------------------- +Timer::Timer() +: startTime(0), endTime(0), running(false) +{ +#ifdef WIN32 + QueryPerformanceFrequency(&frequency); + startCount.QuadPart = 0; + endCount.QuadPart = 0; +#else + startCount.tv_sec = startCount.tv_usec = 0; + endCount.tv_sec = endCount.tv_usec = 0; +#endif +} + + +void Timer::start() +{ +#ifdef WIN32 + QueryPerformanceCounter(&startCount); +#else + gettimeofday(&startCount, NULL); +#endif + running = true; +} + + +void Timer::stop() +{ +#ifdef WIN32 + QueryPerformanceCounter(&endCount); +#else + gettimeofday(&endCount, NULL); +#endif + running = false; +} + + +double Timer::elapsed() +{ +#ifdef WIN32 + if (running) + QueryPerformanceCounter(&endCount); + + startTime = startCount.QuadPart * (1e3 / frequency.QuadPart); + endTime = endCount.QuadPart * (1e3 / frequency.QuadPart); +#else + if(!stopped) + 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 new file mode 100644 index 00000000..2aaf725a --- /dev/null +++ b/FTNoIR_Tracker_PT/timer.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef PT_TIMER_H +#define PT_TIMER_H + +#ifdef WIN32 // Windows system specific +#include +#else // Unix based system specific +#include +#endif + +// ---------------------------------------------------------------------------- +// high resolution timer based on http://www.songho.ca/misc/timer/timer.html +class Timer +{ +public: + Timer(); + + void start(); + void stop(); + void restart() { start(); } // for Qt compatibility + double elapsed(); // get elapsed time in ms + +protected: + double startTime; // starting time in ms + double endTime; // ending time in ms + bool running; + +#ifdef WIN32 + LARGE_INTEGER frequency; // ticks per second + LARGE_INTEGER startCount; + LARGE_INTEGER endCount; +#else + timeval startCount; + timeval endCount; +#endif +}; + +#endif //PT_TIMER_H \ No newline at end of file diff --git a/FTNoIR_Tracker_PT/videoInput/videoInput.h b/FTNoIR_Tracker_PT/videoInput/videoInput.h new file mode 100644 index 00000000..4244902c --- /dev/null +++ b/FTNoIR_Tracker_PT/videoInput/videoInput.h @@ -0,0 +1,385 @@ +#ifndef _VIDEOINPUT +#define _VIDEOINPUT + +//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +//THE SOFTWARE. + +////////////////////////////////////////////////////////// +//Written by Theodore Watson - theo.watson@gmail.com // +//Do whatever you want with this code but if you find // +//a bug or make an improvement I would love to know! // +// // +//Warning This code is experimental // +//use at your own risk :) // +////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////// +/* Shoutouts + +Thanks to: + + Dillip Kumar Kara for crossbar code. + Zachary Lieberman for getting me into this stuff + and for being so generous with time and code. + The guys at Potion Design for helping me with VC++ + Josh Fisher for being a serious C++ nerd :) + Golan Levin for helping me debug the strangest + and slowest bug in the world! + + And all the people using this library who send in + bugs, suggestions and improvements who keep me working on + the next version - yeah thanks a lot ;) + +*/ +///////////////////////////////////////////////////////// + + + +#include +#include +#include +#include +#include + +//this is for TryEnterCriticalSection +#ifndef _WIN32_WINNT + # define _WIN32_WINNT 0x400 +#endif +#include + + +//Example Usage +/* + //create a videoInput object + videoInput VI; + + //Prints out a list of available devices and returns num of devices found + int numDevices = VI.listDevices(); + + int device1 = 0; //this could be any deviceID that shows up in listDevices + int device2 = 1; //this could be any deviceID that shows up in listDevices + + //if you want to capture at a different frame rate (default is 30) + //specify it here, you are not guaranteed to get this fps though. + //VI.setIdealFramerate(dev, 60); + + //setup the first device - there are a number of options: + + VI.setupDevice(device1); //setup the first device with the default settings + //VI.setupDevice(device1, VI_COMPOSITE); //or setup device with specific connection type + //VI.setupDevice(device1, 320, 240); //or setup device with specified video size + //VI.setupDevice(device1, 320, 240, VI_COMPOSITE); //or setup device with video size and connection type + + //VI.setFormat(device1, VI_NTSC_M); //if your card doesn't remember what format it should be + //call this with the appropriate format listed above + //NOTE: must be called after setupDevice! + + //optionally setup a second (or third, fourth ...) device - same options as above + VI.setupDevice(device2); + + //As requested width and height can not always be accomodated + //make sure to check the size once the device is setup + + int width = VI.getWidth(device1); + int height = VI.getHeight(device1); + int size = VI.getSize(device1); + + unsigned char * yourBuffer1 = new unsigned char[size]; + unsigned char * yourBuffer2 = new unsigned char[size]; + + //to get the data from the device first check if the data is new + if(VI.isFrameNew(device1)){ + VI.getPixels(device1, yourBuffer1, false, false); //fills pixels as a BGR (for openCV) unsigned char array - no flipping + VI.getPixels(device1, yourBuffer2, true, true); //fills pixels as a RGB (for openGL) unsigned char array - flipping! + } + + //same applies to device2 etc + + //to get a settings dialog for the device + VI.showSettingsWindow(device1); + + + //Shut down devices properly + VI.stopDevice(device1); + VI.stopDevice(device2); +*/ + + +////////////////////////////////////// VARS AND DEFS ////////////////////////////////// + + +//STUFF YOU CAN CHANGE + +//change for verbose debug info +static bool verbose = true; + +//if you need VI to use multi threaded com +//#define VI_COM_MULTI_THREADED + +//STUFF YOU DON'T CHANGE + +//videoInput defines +#define VI_VERSION 0.1995 +#define VI_MAX_CAMERAS 20 +#define VI_NUM_TYPES 18 //DON'T TOUCH +#define VI_NUM_FORMATS 18 //DON'T TOUCH + +//defines for setPhyCon - tuner is not as well supported as composite and s-video +#define VI_COMPOSITE 0 +#define VI_S_VIDEO 1 +#define VI_TUNER 2 +#define VI_USB 3 +#define VI_1394 4 + +//defines for formats +#define VI_NTSC_M 0 +#define VI_PAL_B 1 +#define VI_PAL_D 2 +#define VI_PAL_G 3 +#define VI_PAL_H 4 +#define VI_PAL_I 5 +#define VI_PAL_M 6 +#define VI_PAL_N 7 +#define VI_PAL_NC 8 +#define VI_SECAM_B 9 +#define VI_SECAM_D 10 +#define VI_SECAM_G 11 +#define VI_SECAM_H 12 +#define VI_SECAM_K 13 +#define VI_SECAM_K1 14 +#define VI_SECAM_L 15 +#define VI_NTSC_M_J 16 +#define VI_NTSC_433 17 + + +//allows us to directShow classes here with the includes in the cpp +struct ICaptureGraphBuilder2; +struct IGraphBuilder; +struct IBaseFilter; +struct IAMCrossbar; +struct IMediaControl; +struct ISampleGrabber; +struct IMediaEventEx; +struct IAMStreamConfig; +struct _AMMediaType; +class SampleGrabberCallback; +typedef _AMMediaType AM_MEDIA_TYPE; + +//keeps track of how many instances of VI are being used +//don't touch +static int comInitCount = 0; + + +//////////////////////////////////////// VIDEO DEVICE /////////////////////////////////// + +class videoDevice{ + + + public: + + videoDevice(); + void setSize(int w, int h); + void NukeDownstream(IBaseFilter *pBF); + void destroyGraph(); + ~videoDevice(); + + int videoSize; + int width; + int height; + int tryWidth; + int tryHeight; + + ICaptureGraphBuilder2 *pCaptureGraph; // Capture graph builder object + IGraphBuilder *pGraph; // Graph builder object + IMediaControl *pControl; // Media control object + IBaseFilter *pVideoInputFilter; // Video Capture filter + IBaseFilter *pGrabberF; + IBaseFilter * pDestFilter; + IAMStreamConfig *streamConf; + ISampleGrabber * pGrabber; // Grabs frame + AM_MEDIA_TYPE * pAmMediaType; + + IMediaEventEx * pMediaEvent; + + GUID videoType; + long formatType; + + SampleGrabberCallback * sgCallback; + + bool tryDiffSize; + bool useCrossbar; + bool readyToCapture; + bool sizeSet; + bool setupStarted; + bool specificFormat; + bool autoReconnect; + int nFramesForReconnect; + unsigned long nFramesRunning; + int connection; + int storeConn; + int myID; + long requestedFrameTime; //ie fps + + char nDeviceName[255]; + WCHAR wDeviceName[255]; + + unsigned char * pixels; + char * pBuffer; + +}; + + + + +////////////////////////////////////// VIDEO INPUT ///////////////////////////////////// + + + +class videoInput{ + + public: + videoInput(); + ~videoInput(); + + //turns off console messages - default is to print messages + static void setVerbose(bool _verbose); + + //Functions in rough order they should be used. + static int listDevices(bool silent = false); + + //needs to be called after listDevices - otherwise returns NULL + static char * getDeviceName(int deviceID); + + //choose to use callback based capture - or single threaded + void setUseCallback(bool useCallback); + + //call before setupDevice + //directshow will try and get the closest possible framerate to what is requested + void setIdealFramerate(int deviceID, int idealFramerate); + + //some devices will stop delivering frames after a while - this method gives you the option to try and reconnect + //to a device if videoInput detects that a device has stopped delivering frames. + //you MUST CALL isFrameNew every app loop for this to have any effect + void setAutoReconnectOnFreeze(int deviceNumber, bool doReconnect, int numMissedFramesBeforeReconnect); + + //Choose one of these four to setup your device + bool setupDevice(int deviceID); + bool setupDevice(int deviceID, int w, int h); + + //These two are only for capture cards + //USB and Firewire cameras souldn't specify connection + bool setupDevice(int deviceID, int connection); + bool setupDevice(int deviceID, int w, int h, int connection); + + //If you need to you can set your NTSC/PAL/SECAM + //preference here. if it is available it will be used. + //see #defines above for available formats - eg VI_NTSC_M or VI_PAL_B + //should be called after setupDevice + //can be called multiple times + bool setFormat(int deviceNumber, int format); + + //Tells you when a new frame has arrived - you should call this if you have specified setAutoReconnectOnFreeze to true + bool isFrameNew(int deviceID); + + bool isDeviceSetup(int deviceID); + + //Returns the pixels - flipRedAndBlue toggles RGB/BGR flipping - and you can flip the image too + unsigned char * getPixels(int deviceID, bool flipRedAndBlue = true, bool flipImage = false); + + //Or pass in a buffer for getPixels to fill returns true if successful. + bool getPixels(int id, unsigned char * pixels, bool flipRedAndBlue = true, bool flipImage = false); + + //Launches a pop up settings window + //For some reason in GLUT you have to call it twice each time. + void showSettingsWindow(int deviceID); + + //Manual control over settings thanks..... + //These are experimental for now. + bool setVideoSettingFilter(int deviceID, long Property, long lValue, long Flags = NULL, bool useDefaultValue = false); + bool setVideoSettingFilterPct(int deviceID, long Property, float pctValue, long Flags = NULL); + bool getVideoSettingFilter(int deviceID, long Property, long &min, long &max, long &SteppingDelta, long ¤tValue, long &flags, long &defaultValue); + + bool setVideoSettingCamera(int deviceID, long Property, long lValue, long Flags = NULL, bool useDefaultValue = false); + bool setVideoSettingCameraPct(int deviceID, long Property, float pctValue, long Flags = NULL); + bool getVideoSettingCamera(int deviceID, long Property, long &min, long &max, long &SteppingDelta, long ¤tValue, long &flags, long &defaultValue); + + //bool setVideoSettingCam(int deviceID, long Property, long lValue, long Flags = NULL, bool useDefaultValue = false); + + //get width, height and number of pixels + int getWidth(int deviceID); + int getHeight(int deviceID); + int getSize(int deviceID); + + //completely stops and frees a device + void stopDevice(int deviceID); + + //as above but then sets it up with same settings + bool restartDevice(int deviceID); + + //number of devices available + int devicesFound; + + long propBrightness; + long propContrast; + long propHue; + long propSaturation; + long propSharpness; + long propGamma; + long propColorEnable; + long propWhiteBalance; + long propBacklightCompensation; + long propGain; + + long propPan; + long propTilt; + long propRoll; + long propZoom; + long propExposure; + long propIris; + long propFocus; + + + private: + void setPhyCon(int deviceID, int conn); + void setAttemptCaptureSize(int deviceID, int w, int h); + bool setup(int deviceID); + void processPixels(unsigned char * src, unsigned char * dst, int width, int height, bool bRGB, bool bFlip); + int start(int deviceID, videoDevice * VD); + int getDeviceCount(); + void getMediaSubtypeAsString(GUID type, char * typeAsString); + + HRESULT getDevice(IBaseFilter **pSrcFilter, int deviceID, WCHAR * wDeviceName, char * nDeviceName); + static HRESULT ShowFilterPropertyPages(IBaseFilter *pFilter); + HRESULT SaveGraphFile(IGraphBuilder *pGraph, WCHAR *wszPath); + HRESULT routeCrossbar(ICaptureGraphBuilder2 **ppBuild, IBaseFilter **pVidInFilter, int conType, GUID captureMode); + + //don't touch + static bool comInit(); + static bool comUnInit(); + + int connection; + int callbackSetCount; + bool bCallback; + + GUID CAPTURE_MODE; + + //Extra video subtypes + GUID MEDIASUBTYPE_Y800; + GUID MEDIASUBTYPE_Y8; + GUID MEDIASUBTYPE_GREY; + + videoDevice * VDList[VI_MAX_CAMERAS]; + GUID mediaSubtypes[VI_NUM_TYPES]; + long formatTypes[VI_NUM_FORMATS]; + + static void __cdecl basicThread(void * objPtr); + + static char deviceNames[VI_MAX_CAMERAS][255]; + +}; + + #endif diff --git a/FTNoIR_Tracker_PT/videoInput/videoInput_vc8.lib b/FTNoIR_Tracker_PT/videoInput/videoInput_vc8.lib new file mode 100644 index 00000000..15ca9cf3 Binary files /dev/null and b/FTNoIR_Tracker_PT/videoInput/videoInput_vc8.lib differ diff --git a/FTNoIR_Tracker_PT/videoInput/videoInput_vc9.lib b/FTNoIR_Tracker_PT/videoInput/videoInput_vc9.lib new file mode 100644 index 00000000..32637cc9 Binary files /dev/null and b/FTNoIR_Tracker_PT/videoInput/videoInput_vc9.lib differ -- cgit v1.2.3