From 3089c4bbc10e98d18f43e8a70e7a3d0c0eaf0900 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 22 Mar 2013 20:48:17 +0100 Subject: Downcase. PLEASE TURN OFF IGNORING CASE IN GIT CONFIG!!! .git/config: [core] ignorecase = false --- ftnoir_tracker_pt/FTNoIR_PT_Controls.ui | 1764 ++++++++++++++++++++++ ftnoir_tracker_pt/FTNoIR_Tracker_PT.rc | 61 + ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc8.vcproj | 559 +++++++ ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc9.vcproj | 524 +++++++ ftnoir_tracker_pt/Resources/Logo_IR.png | Bin 0 -> 10386 bytes 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/Resources/icon.ico | Bin 0 -> 4286 bytes ftnoir_tracker_pt/camera.cpp | 226 +++ ftnoir_tracker_pt/camera.h | 116 ++ ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 257 ++++ ftnoir_tracker_pt/ftnoir_tracker_pt.h | 86 ++ ftnoir_tracker_pt/ftnoir_tracker_pt.qrc | 10 + ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 336 +++++ ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h | 102 ++ ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp | 40 + ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h | 19 + ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp | 150 ++ ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 84 ++ ftnoir_tracker_pt/point_extractor.cpp | 97 ++ ftnoir_tracker_pt/point_extractor.h | 31 + ftnoir_tracker_pt/point_tracker.cpp | 352 +++++ ftnoir_tracker_pt/point_tracker.h | 114 ++ ftnoir_tracker_pt/resource.h | 14 + ftnoir_tracker_pt/timer.cpp | 66 + ftnoir_tracker_pt/timer.h | 44 + ftnoir_tracker_pt/trans_calib.cpp | 44 + ftnoir_tracker_pt/trans_calib.h | 39 + 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 ftnoir_tracker_pt/video_widget.cpp | 98 ++ ftnoir_tracker_pt/video_widget.h | 40 + 35 files changed, 5658 insertions(+) create mode 100644 ftnoir_tracker_pt/FTNoIR_PT_Controls.ui create mode 100644 ftnoir_tracker_pt/FTNoIR_Tracker_PT.rc create mode 100644 ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc8.vcproj create mode 100644 ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc9.vcproj create mode 100644 ftnoir_tracker_pt/Resources/Logo_IR.png 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/Resources/icon.ico create mode 100644 ftnoir_tracker_pt/camera.cpp create mode 100644 ftnoir_tracker_pt/camera.h create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt.cpp create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt.h create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt.qrc create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp create mode 100644 ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h create mode 100644 ftnoir_tracker_pt/point_extractor.cpp create mode 100644 ftnoir_tracker_pt/point_extractor.h create mode 100644 ftnoir_tracker_pt/point_tracker.cpp create mode 100644 ftnoir_tracker_pt/point_tracker.h create mode 100644 ftnoir_tracker_pt/resource.h create mode 100644 ftnoir_tracker_pt/timer.cpp create mode 100644 ftnoir_tracker_pt/timer.h create mode 100644 ftnoir_tracker_pt/trans_calib.cpp create mode 100644 ftnoir_tracker_pt/trans_calib.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 create mode 100644 ftnoir_tracker_pt/video_widget.cpp create mode 100644 ftnoir_tracker_pt/video_widget.h (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui new file mode 100644 index 00000000..0174df23 --- /dev/null +++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui @@ -0,0 +1,1764 @@ + + + UICPTClientControls + + + Qt::ApplicationModal + + + + 0 + 0 + 405 + 489 + + + + + 0 + 0 + + + + PointTracker Settings + + + + :/Resources/icon.ico:/Resources/icon.ico + + + Qt::LeftToRight + + + false + + + + QLayout::SetFixedSize + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + + + 0 + + + + General + + + + + + Tracker Thread + + + + + + + + + + + Dynamic Pose Resolution + + + + + + + Sleep time + + + sleep_spin + + + + + + + + + Time the tracker thread sleeps after each processed frame + + + + + + 9999 + + + + + + + ms + + + + + + + + + Auto-reset time + + + reset_spin + + + + + + + + + Time until automatic reset of tracker's internal state when no valid tracking result is found + + + 9999 + + + + + + + ms + + + + + + + + + Whether to update the content of the VideoWidget + + + Show VideoWidget + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + false + + + Reset the tracker's internal state + + + Reset + + + + + + + + + + + + + 0 + 85 + + + + Enable Axis + + + + + + + + Roll: + + + chkEnableRoll + + + + + + + Pitch: + + + chkEnablePitch + + + + + + + Yaw: + + + chkEnableYaw + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + X: + + + chkEnableX + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + Y: + + + chkEnableY + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + Z: + + + chkEnableZ + + + + + + + + 20 + 16777215 + + + + Qt::LeftToRight + + + + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + Status + + + + + + QFormLayout::AllNonFixedFieldsGrow + + + + + Camera Info: + + + + + + + + 0 + 0 + + + + + 120 + 0 + + + + + + + + + + + Extracted Points: + + + + + + + + 50 + 0 + + + + + + + + + + + + + + + + + Camera + + + + + + Camera Settings + + + + + + + + + + Index + + + camindex_spin + + + + + + + Capture device index + + + + + + + FPS + + + fps_spin + + + + + + + Desired capture framerate + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + Camera Pitch (upwards = positive) + + + campitch_spin + + + + + + + Qt::DefaultContextMenu + + + The angle the camera is facing upwards + + + -99 + + + + + + + deg + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + + + + + + Point Extraction + + + + + + + + Threshold + + + threshold_slider + + + + + + + Intensity threshold for point extraction + + + 255 + + + 127 + + + Qt::Horizontal + + + + + + + + + + + Min Diameter + + + mindiam_spin + + + + + + + Minimum point diameter + + + + + + + px + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Max Diameter + + + maxdiam_spin + + + + + + + Maximum point diameter + + + + + + + px + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Status + + + + + + QFormLayout::AllNonFixedFieldsGrow + + + + + Camera Info: + + + + + + + + 0 + 0 + + + + + 120 + 0 + + + + + + + + + + + Extracted Points: + + + + + + + + 50 + 0 + + + + + + + + + + + + + + + + + Model + + + + + + QTabWidget::Rounded + + + 2 + + + false + + + false + + + false + + + + Clip + + + + + + Model Dimensions (mm) + + + + + + + 0 + 0 + + + + + 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 + + + + + + + + + 0 + 0 + + + + + 100 + 140 + + + + + + 10 + 10 + 46 + 13 + + + + Front + + + + + + 40 + 30 + 21 + 111 + + + + + + + :/Resources/clip_front.png + + + + + + 60 + 70 + 16 + 16 + + + + R + + + + + + + + + + + + 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 + + + + + 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) + + + + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p></body></html> + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + + + 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 + + + + + + + + + + Qt::Vertical + + + + 20 + 20 + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + -999 + + + 999 + + + + + + + x: + + + + + + + y: + + + + + + + z: + + + + + + + -999 + + + 999 + + + + + + + -999 + + + 999 + + + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + false + + + Calibrate + + + true + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + + + + + + + + + About + + + + + 30 + 30 + 161 + 111 + + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.0</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 + + + + + + 200 + 30 + 141 + 141 + + + + + + + :/Resources/Logo_IR.png + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Ok + + + + + + + Cancel + + + + + + + + + tabWidget + sleep_spin + reset_spin + chkEnableRoll + chkEnablePitch + chkEnableYaw + chkEnableX + chkEnableY + chkEnableZ + camindex_spin + res_x_spin + res_y_spin + fps_spin + f_dspin + campitch_spin + threshold_slider + mindiam_spin + maxdiam_spin + 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 + m2x_spin + m2y_spin + m2z_spin + tx_spin + ty_spin + tz_spin + tcalib_button + ok_button + cancel_button + + + + + + + dynpose_check + toggled(bool) + reset_spin + setEnabled(bool) + + + 285 + 105 + + + 139 + 111 + + + + + + startEngineClicked() + stopEngineClicked() + cameraSettingsClicked() + + diff --git a/ftnoir_tracker_pt/FTNoIR_Tracker_PT.rc b/ftnoir_tracker_pt/FTNoIR_Tracker_PT.rc new file mode 100644 index 00000000..11c5d52f --- /dev/null +++ b/ftnoir_tracker_pt/FTNoIR_Tracker_PT.rc @@ -0,0 +1,61 @@ +// Microsoft Visual C++ generated resource script. +// +#include "resource.h" + +#define APSTUDIO_READONLY_SYMBOLS +///////////////////////////////////////////////////////////////////////////// +// +// Generated from the TEXTINCLUDE 2 resource. +// +#include "afxres.h" + +///////////////////////////////////////////////////////////////////////////// +#undef APSTUDIO_READONLY_SYMBOLS + +///////////////////////////////////////////////////////////////////////////// +// English (U.S.) resources + +#if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_ENU) +LANGUAGE 9, 1 +#pragma code_page(1252) + +#ifdef APSTUDIO_INVOKED +///////////////////////////////////////////////////////////////////////////// +// +// TEXTINCLUDE +// + +1 TEXTINCLUDE +BEGIN + "resource.h\0" +END + +2 TEXTINCLUDE +BEGIN + "#include ""afxres.h""\r\n" + "\0" +END + +3 TEXTINCLUDE +BEGIN + "\r\n" + "\0" +END + +#endif // APSTUDIO_INVOKED + +#endif // English (U.S.) resources +///////////////////////////////////////////////////////////////////////////// + + + +#ifndef APSTUDIO_INVOKED +///////////////////////////////////////////////////////////////////////////// +// +// Generated from the TEXTINCLUDE 3 resource. +// + + +///////////////////////////////////////////////////////////////////////////// +#endif // not APSTUDIO_INVOKED + diff --git a/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc8.vcproj b/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc8.vcproj new file mode 100644 index 00000000..5b70b459 --- /dev/null +++ b/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc8.vcproj @@ -0,0 +1,559 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc9.vcproj b/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc9.vcproj new file mode 100644 index 00000000..95f5ac62 --- /dev/null +++ b/ftnoir_tracker_pt/FTNoIR_Tracker_PT_vc9.vcproj @@ -0,0 +1,524 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ftnoir_tracker_pt/Resources/Logo_IR.png b/ftnoir_tracker_pt/Resources/Logo_IR.png new file mode 100644 index 00000000..95032a25 Binary files /dev/null and b/ftnoir_tracker_pt/Resources/Logo_IR.png differ diff --git a/ftnoir_tracker_pt/Resources/cap_front.png b/ftnoir_tracker_pt/Resources/cap_front.png new file mode 100644 index 00000000..14207a67 Binary files /dev/null and b/ftnoir_tracker_pt/Resources/cap_front.png differ diff --git a/ftnoir_tracker_pt/Resources/cap_side.png b/ftnoir_tracker_pt/Resources/cap_side.png new file mode 100644 index 00000000..5ad4ee65 Binary files /dev/null and b/ftnoir_tracker_pt/Resources/cap_side.png differ diff --git a/ftnoir_tracker_pt/Resources/clip_front.png b/ftnoir_tracker_pt/Resources/clip_front.png new file mode 100644 index 00000000..04880138 Binary files /dev/null and b/ftnoir_tracker_pt/Resources/clip_front.png differ diff --git a/ftnoir_tracker_pt/Resources/clip_side.png b/ftnoir_tracker_pt/Resources/clip_side.png new file mode 100644 index 00000000..72667ac7 Binary files /dev/null and b/ftnoir_tracker_pt/Resources/clip_side.png differ diff --git a/ftnoir_tracker_pt/Resources/icon.ico b/ftnoir_tracker_pt/Resources/icon.ico new file mode 100644 index 00000000..c4b2aedc Binary files /dev/null and b/ftnoir_tracker_pt/Resources/icon.ico differ diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp new file mode 100644 index 00000000..fc11c738 --- /dev/null +++ b/ftnoir_tracker_pt/camera.cpp @@ -0,0 +1,226 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "camera.h" +#include + +using namespace cv; + +// ---------------------------------------------------------------------------- +void Camera::set_index(int index) +{ + if (desired_index != index) + { + desired_index = index; + _set_index(); + + // reset fps + dt_valid = 0; + dt_mean = 0; + active_index = index; + } +} + +void Camera::set_f(float f) +{ + if (cam_desired.f != f) + { + cam_desired.f = f; + _set_f(); + } +} +void Camera::set_fps(int fps) +{ + if (cam_desired.fps != fps) + { + cam_desired.fps = fps; + _set_fps(); + } +} + +void Camera::set_res(int x_res, int y_res) +{ + if (cam_desired.res_x != x_res || cam_desired.res_y != y_res) + { + cam_desired.res_x = x_res; + cam_desired.res_y = y_res; + _set_res(); + } +} + +bool Camera::get_frame(float dt, cv::Mat* frame) +{ + bool new_frame = _get_frame(frame); + // measure fps of valid frames + const float dt_smoothing_const = 0.9; + dt_valid += dt; + if (new_frame) + { + dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid; + cam_info.fps = 1.0 / dt_mean; + dt_valid = 0; + } + return new_frame; +} + +// ---------------------------------------------------------------------------- +/* +void CVCamera::start() +{ + cap = cvCreateCameraCapture(desired_index); + // extract camera info + if (cap) + { + 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); + } +} + +void CVCamera::stop() +{ + if (cap) cvReleaseCapture(&cap); + active = false; +} + +bool CVCamera::_get_frame(Mat* frame) +{ + if (cap && cvGrabFrame(cap) != 0) + { + // retrieve frame + IplImage* _img = cvRetrieveFrame(cap, 0); + if(_img) + { + if(_img->origin == IPL_ORIGIN_TL) + *frame = Mat(_img); + else + { + Mat temp(_img); + flip(temp, *frame, 0); + } + return true; + } + } + return false; +} + +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) + { + 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(); +} + diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h new file mode 100644 index 00000000..cd1f0842 --- /dev/null +++ b/ftnoir_tracker_pt/camera.h @@ -0,0 +1,116 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include "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() : 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); + 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; + + bool active; + int desired_index; + int active_index; + 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 new file mode 100644 index 00000000..5b77da69 --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -0,0 +1,257 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt.h" +#include +#include +#include +#include +#include + +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), tracking_valid(false) +{ + qDebug()<<"Tracker::Tracker"; + TrackerSettings settings; + settings.load_ini(); + apply(settings); + camera.start(); + start(); +} + +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 (commands & ABORT) break; + if (commands & PAUSE) continue; + commands = 0; + + dt = time.elapsed() / 1000.0; + time.restart(); + + new_frame = camera.get_frame(dt, &frame); + if (new_frame && !frame.empty()) + { + const std::vector& points = point_extractor.extract_points(frame, dt, draw_frame); + tracking_valid = point_tracker.track(points, camera.get_info().f, dt); + frame_count++; + } +#ifdef PT_PERF_LOG + log_stream<<"dt: "<(new PointModel(settings.M01, settings.M02)); + point_tracker.dynamic_pose_resolution = settings.dyn_pose_res; + sleep_time = settings.sleep_time; + point_tracker.dt_reset = settings.reset_time / 1000.0; + draw_frame = settings.video_widget; + cam_pitch = settings.cam_pitch; + + bEnableRoll = settings.bEnableRoll; + bEnablePitch = settings.bEnablePitch; + bEnableYaw = settings.bEnableYaw; + bEnableX = settings.bEnableX; + bEnableY = settings.bEnableY; + bEnableZ = settings.bEnableZ; + + t_MH = settings.t_MH; + qDebug()<<"Tracker::apply ends"; +} + +void Tracker::reset() +{ + QMutexLocker lock(&mutex); + point_tracker.reset(); +} + +void Tracker::center() +{ + point_tracker.reset(); // we also do a reset here since there is no reset shortkey yet + QMutexLocker lock(&mutex); + FrameTrafo X_CM_0 = point_tracker.get_pose(); + FrameTrafo X_MH(Matx33f::eye(), t_MH); + X_CH_0 = X_CM_0 * X_MH; +} + +//----------------------------------------------------------------------------- +// ITracker interface +void Tracker::Initialize(QFrame *videoframe) +{ + const int VIDEO_FRAME_WIDTH = 252; + const int VIDEO_FRAME_HEIGHT = 189; + + qDebug("Tracker::Initialize"); + // setup video frame + videoframe->setAttribute(Qt::WA_NativeWindow); + videoframe->show(); + 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); +} + +void Tracker::refreshVideo() +{ + if (video_widget) + { + Mat frame_copy; + shared_ptr< vector > points; + { + QMutexLocker lock(&mutex); + if (!draw_frame || frame.empty()) return; + + // copy the frame and points from the tracker thread + frame_copy = frame.clone(); + points = shared_ptr< vector >(new vector(point_extractor.get_points())); + } + + video_widget->update(frame_copy, points); + } +} + +void Tracker::StartTracker(HWND parent_window) +{ + reset_command(PAUSE); +} + +void Tracker::StopTracker(bool exit) +{ + set_command(PAUSE); +} + +bool Tracker::GiveHeadPoseData(THeadPoseData *data) +{ + const float rad2deg = 180.0/3.14159265; + const float 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->x = t[0] / 10.0; // convert to cm + } + if (bEnableY) { + data->y = t[1] / 10.0; + } + if (bEnableZ) { + data->z = 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 + float 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") + +FTNOIR_TRACKER_BASE_EXPORT ITrackerPtr __stdcall GetTracker() +{ + return new Tracker; +} \ No newline at end of file diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h new file mode 100644 index 00000000..2533a39b --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -0,0 +1,86 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef FTNOIR_TRACKER_PT_H +#define FTNOIR_TRACKER_PT_H + +#include "..\ftnoir_tracker_base\ftnoir_tracker_base.h" +#include "ftnoir_tracker_pt_settings.h" +#include "camera.h" +#include "point_extractor.h" +#include "point_tracker.h" +#include "video_widget.h" +#include "timer.h" + +#include +#include +#include +#include + +//----------------------------------------------------------------------------- +class Tracker : public ITracker, QThread +{ +public: + Tracker(); + ~Tracker(); + + // ITracker interface + void Initialize(QFrame *videoframe); + void StartTracker(HWND parent_window); + void StopTracker(bool exit); + bool GiveHeadPoseData(THeadPoseData *data); + + void refreshVideo(); + + void apply(const TrackerSettings& settings); + void center(); + void reset(); // reset the trackers internal state variables + void run(); + + 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 + + QMutex mutex; + 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); + int commands; + + VICamera 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; + + VideoWidget* video_widget; + 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 new file mode 100644 index 00000000..eb1fba2c --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.qrc @@ -0,0 +1,10 @@ + + + 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 new file mode 100644 index 00000000..a1531dd7 --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -0,0 +1,336 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt_dialog.h" + +#include +#include + +//----------------------------------------------------------------------------- +TrackerDialog::TrackerDialog() + : settings_dirty(false), tracker(NULL), timer(this), trans_calib_running(false) +{ + qDebug()<<"TrackerDialog::TrackerDialog"; + setAttribute(Qt::WA_DeleteOnClose, false); + + ui.setupUi( this ); + + settings.load_ini(); + dialog_settings.load_ini(); + + // initialize ui values + ui.videowidget_check->setChecked(settings.video_widget); + ui.dynpose_check->setChecked(settings.dyn_pose_res); + ui.sleep_spin->setValue(settings.sleep_time); + ui.reset_spin->setValue(settings.reset_time); + ui.camindex_spin->setValue(settings.cam_index); + ui.f_dspin->setValue(settings.cam_f); + ui.res_x_spin->setValue(settings.cam_res_x); + ui.res_y_spin->setValue(settings.cam_res_y); + ui.fps_spin->setValue(settings.cam_fps); + ui.campitch_spin->setValue(settings.cam_pitch); + ui.threshold_slider->setValue(settings.threshold); + + ui.chkEnableRoll->setChecked(settings.bEnableRoll); + ui.chkEnablePitch->setChecked(settings.bEnablePitch); + ui.chkEnableYaw->setChecked(settings.bEnableYaw); + ui.chkEnableX->setChecked(settings.bEnableX); + ui.chkEnableY->setChecked(settings.bEnableY); + ui.chkEnableZ->setChecked(settings.bEnableZ); + + ui.mindiam_spin->setValue(settings.min_point_size); + ui.maxdiam_spin->setValue(settings.max_point_size); + ui.model_tabs->setCurrentIndex(dialog_settings.active_model_panel); + ui.clip_bheight_spin->setValue(dialog_settings.clip_by); + ui.clip_blength_spin->setValue(dialog_settings.clip_bz); + ui.clip_theight_spin->setValue(dialog_settings.clip_ty); + ui.clip_tlength_spin->setValue(dialog_settings.clip_tz); + ui.cap_width_spin->setValue(dialog_settings.cap_x); + ui.cap_height_spin->setValue(dialog_settings.cap_y); + ui.cap_length_spin->setValue(dialog_settings.cap_z); + ui.m1x_spin->setValue(dialog_settings.M01x); + ui.m1y_spin->setValue(dialog_settings.M01y); + ui.m1z_spin->setValue(dialog_settings.M01z); + ui.m2x_spin->setValue(dialog_settings.M02x); + ui.m2y_spin->setValue(dialog_settings.M02y); + ui.m2z_spin->setValue(dialog_settings.M02z); + ui.tx_spin->setValue(settings.t_MH[0]); + ui.ty_spin->setValue(settings.t_MH[1]); + ui.tz_spin->setValue(settings.t_MH[2]); + + // connect Qt signals and slots + connect( ui.videowidget_check,SIGNAL(toggled(bool)), this,SLOT(set_video_widget(bool)) ); + connect( ui.dynpose_check,SIGNAL(toggled(bool)), this,SLOT(set_dyn_pose_res(bool)) ); + connect( ui.sleep_spin,SIGNAL(valueChanged(int)), this,SLOT(set_sleep_time(int)) ); + connect( ui.reset_spin,SIGNAL(valueChanged(int)), this,SLOT(set_reset_time(int)) ); + connect( ui.camindex_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_index(int)) ); + connect( ui.f_dspin,SIGNAL(valueChanged(double)), this,SLOT(set_cam_f(double)) ); + connect( ui.res_x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_x(int)) ); + connect( ui.res_y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_res_y(int)) ); + connect( ui.fps_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_fps(int)) ); + connect( ui.campitch_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cam_pitch(int)) ); + connect( ui.threshold_slider,SIGNAL(sliderMoved(int)), this,SLOT(set_threshold(int)) ); + + connect( ui.chkEnableRoll,SIGNAL(toggled(bool)), this,SLOT(set_ena_roll(bool)) ); + connect( ui.chkEnablePitch,SIGNAL(toggled(bool)), this,SLOT(set_ena_pitch(bool)) ); + connect( ui.chkEnableYaw,SIGNAL(toggled(bool)), this,SLOT(set_ena_yaw(bool)) ); + connect( ui.chkEnableX,SIGNAL(toggled(bool)), this,SLOT(set_ena_x(bool)) ); + connect( ui.chkEnableY,SIGNAL(toggled(bool)), this,SLOT(set_ena_y(bool)) ); + connect( ui.chkEnableZ,SIGNAL(toggled(bool)), this,SLOT(set_ena_z(bool)) ); + + connect( ui.mindiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_min_point_size(int)) ); + connect( ui.maxdiam_spin,SIGNAL(valueChanged(int)), this,SLOT(set_max_point_size(int)) ); + connect( ui.model_tabs,SIGNAL(currentChanged(int)), this,SLOT(set_model(int)) ); + connect( ui.clip_theight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_height(int)) ); + connect( ui.clip_tlength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_t_length(int)) ); + connect( ui.clip_bheight_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_height(int)) ); + connect( ui.clip_blength_spin,SIGNAL(valueChanged(int)), this,SLOT(set_clip_b_length(int)) ); + connect( ui.cap_width_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_width(int)) ); + connect( ui.cap_height_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_height(int)) ); + connect( ui.cap_length_spin,SIGNAL(valueChanged(int)), this,SLOT(set_cap_length(int)) ); + connect( ui.m1x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1x(int)) ); + connect( ui.m1y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1y(int)) ); + connect( ui.m1z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m1z(int)) ); + connect( ui.m2x_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2x(int)) ); + connect( ui.m2y_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2y(int)) ); + connect( ui.m2z_spin,SIGNAL(valueChanged(int)), this,SLOT(set_m2z(int)) ); + connect( ui.tx_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tx(int)) ); + connect( ui.ty_spin,SIGNAL(valueChanged(int)), this,SLOT(set_ty(int)) ); + connect( ui.tz_spin,SIGNAL(valueChanged(int)), this,SLOT(set_tz(int)) ); + + connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) ); + + connect(ui.reset_button, SIGNAL(clicked()), this, SLOT(doReset())); + //connect(ui.center_button, SIGNAL(clicked()), this, SLOT(doCenter())); + + connect(ui.ok_button, SIGNAL(clicked()), this, SLOT(doOK())); + connect(ui.cancel_button, SIGNAL(clicked()), this, SLOT(doCancel())); + + connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info())); + timer.start(100); +} + +TrackerDialog::~TrackerDialog() +{ + qDebug()<<"TrackerDialog::~TrackerDialog"; +} + +void TrackerDialog::set_clip() +{ + settings.M01[0] = 0; + settings.M01[1] = dialog_settings.clip_ty; + settings.M01[2] = -dialog_settings.clip_tz; + settings.M02[0] = 0; + settings.M02[1] = -dialog_settings.clip_by; + settings.M02[2] = -dialog_settings.clip_bz; + + settings_changed(); +} + +void TrackerDialog::set_cap() +{ + settings.M01[0] = -dialog_settings.cap_x; + settings.M01[1] = -dialog_settings.cap_y; + settings.M01[2] = -dialog_settings.cap_z; + settings.M02[0] = dialog_settings.cap_x; + settings.M02[1] = -dialog_settings.cap_y; + settings.M02[2] = -dialog_settings.cap_z; + + settings_changed(); +} + +void TrackerDialog::set_custom() +{ + settings.M01[0] = dialog_settings.M01x; + settings.M01[1] = dialog_settings.M01y; + settings.M01[2] = dialog_settings.M01z; + settings.M02[0] = dialog_settings.M02x; + settings.M02[1] = dialog_settings.M02y; + settings.M02[2] = dialog_settings.M02z; + + settings_changed(); +} + +void TrackerDialog::set_model(int val) +{ + dialog_settings.active_model_panel = val; + + switch (val) { + + case TrackerDialogSettings::MODEL_CLIP: + set_clip(); + break; + + case TrackerDialogSettings::MODEL_CAP: + set_cap(); + break; + + case TrackerDialogSettings::MODEL_CUSTOM: + set_custom(); + break; + + default: + break; + } +} + +void TrackerDialog::startstop_trans_calib(bool start) +{ + if (start) + { + qDebug()<<"TrackerDialog:: Starting translation calibration"; + trans_calib.reset(); + trans_calib_running = true; + } + else + { + qDebug()<<"TrackerDialog:: Stoppping translation calibration"; + trans_calib_running = false; + settings.t_MH = trans_calib.get_estimate(); + settings_changed(); + } +} + +void TrackerDialog::trans_calib_step() +{ + if (tracker) + { + FrameTrafo X_CM; + tracker->get_pose(&X_CM); + trans_calib.update(X_CM.R, X_CM.t); + cv::Vec3f t_MH = trans_calib.get_estimate(); + //qDebug()<<"TrackerDialog:: Current translation estimate: "<setValue(t_MH[0]); + ui.ty_spin->setValue(t_MH[1]); + ui.tz_spin->setValue(t_MH[2]); + } +} + +void TrackerDialog::settings_changed() +{ + settings_dirty = true; + if (tracker) tracker->apply(settings); +} + +void TrackerDialog::doCenter() +{ + if (tracker) tracker->center(); +} + +void TrackerDialog::doReset() +{ + if (tracker) tracker->reset(); +} + +void TrackerDialog::doOK() +{ + settings.save_ini(); + dialog_settings.save_ini(); + close(); +} + +void TrackerDialog::doCancel() +{ + if (settings_dirty) { + int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", + QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard ); + switch (ret) { + case QMessageBox::Save: + settings.save_ini(); + dialog_settings.save_ini(); + close(); + break; + case QMessageBox::Discard: + close(); + break; + case QMessageBox::Cancel: + // Cancel was clicked + break; + default: + // should never be reached + break; + } + } + else { + close(); + } +} + +void TrackerDialog::poll_tracker_info() +{ + if (tracker) + { + QString to_print; + + // display caminfo + CamInfo info; + tracker->get_cam_info(&info); + to_print = QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS"; + ui.caminfo_label->setText(to_print); + ui.caminfo_label_2->setText(to_print); + + // display pointinfo + int n_points = tracker->get_n_points(); + to_print = QString::number(n_points); + if (n_points == 3) + to_print += " OK!"; + else + to_print += " BAD!"; + ui.pointinfo_label->setText(to_print); + ui.pointinfo_label_2->setText(to_print); + + // update calibration + if (trans_calib_running) trans_calib_step(); + } + else + { + QString to_print = "Tracker offline"; + ui.caminfo_label->setText(to_print); + ui.caminfo_label_2->setText(to_print); + ui.pointinfo_label->setText(to_print); + ui.pointinfo_label_2->setText(to_print); + } +} + + +//----------------------------------------------------------------------------- +// ITrackerDialog interface +void TrackerDialog::Initialize(QWidget *parent) +{ + QPoint offsetpos(200, 200); + if (parent) { + this->move(parent->pos() + offsetpos); + } + show(); +} + +void TrackerDialog::registerTracker(ITracker *t) +{ + qDebug()<<"TrackerDialog:: Tracker registered"; + tracker = static_cast(t); + if (isVisible() && settings_dirty) tracker->apply(settings); + ui.tcalib_button->setEnabled(true); + //ui.center_button->setEnabled(true); + ui.reset_button->setEnabled(true); +} + +void TrackerDialog::unRegisterTracker() +{ + qDebug()<<"TrackerDialog:: Tracker un-registered"; + tracker = NULL; + ui.tcalib_button->setEnabled(false); + //ui.center_button->setEnabled(false); + ui.reset_button->setEnabled(false); +} + +//----------------------------------------------------------------------------- +#pragma comment(linker, "/export:GetTrackerDialog=_GetTrackerDialog@0") + +FTNOIR_TRACKER_BASE_EXPORT ITrackerDialogPtr __stdcall GetTrackerDialog( ) +{ + return new TrackerDialog; +} \ No newline at end of file diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h new file mode 100644 index 00000000..0f836dfe --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef FTNOIR_TRACKER_PT_DIALOG_H +#define FTNOIR_TRACKER_PT_DIALOG_H + +#include "..\ftnoir_tracker_base\ftnoir_tracker_base.h" +#include "ftnoir_tracker_pt_settings.h" +#include "ftnoir_tracker_pt.h" +#include "ui_FTNoIR_PT_Controls.h" +#include "trans_calib.h" + +#include + +//----------------------------------------------------------------------------- +class TrackerDialog : public QWidget, Ui::UICPTClientControls, 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 \ No newline at end of file diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp new file mode 100644 index 00000000..7f58d77d --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.cpp @@ -0,0 +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 + +//----------------------------------------------------------------------------- +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.ico"); +} + + +//----------------------------------------------------------------------------- +#pragma comment(linker, "/export:GetTrackerDll=_GetTrackerDll@0") + +FTNOIR_TRACKER_BASE_EXPORT ITrackerDllPtr __stdcall GetTrackerDll() +{ + return new TrackerDll; +} diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h new file mode 100644 index 00000000..15ad63aa --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dll.h @@ -0,0 +1,19 @@ +/* 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" + +//----------------------------------------------------------------------------- +class TrackerDll : public ITrackerDll +{ + // ITrackerDll interface + void Initialize() {} + void getFullName(QString *strToBeFilled); + void getShortName(QString *strToBeFilled); + void getDescription(QString *strToBeFilled); + void getIcon(QIcon *icon); +}; \ No newline at end of file diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp new file mode 100644 index 00000000..40d1bc4f --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.cpp @@ -0,0 +1,150 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt.h" +#include +#include + +//----------------------------------------------------------------------------- +void TrackerSettings::load_ini() +{ + qDebug("TrackerSettings::load_ini()"); + + QSettings settings("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( "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("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 ( "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("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 new file mode 100644 index 00000000..88162c86 --- /dev/null +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h @@ -0,0 +1,84 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef FTNOIR_TRACKER_PT_SETTINGS_H +#define FTNOIR_TRACKER_PT_SETTINGS_H + +#include +#include "point_tracker.h" + + +//----------------------------------------------------------------------------- +struct TrackerSettings +{ + // camera + int cam_index; + float cam_f; + int cam_res_x; + int cam_res_y; + int cam_fps; + int cam_pitch; + + // point extraction + int threshold; + int min_point_size; + int max_point_size; + + // point tracking + cv::Vec3f M01; + cv::Vec3f M02; + bool dyn_pose_res; + + // head to model translation + cv::Vec3f t_MH; + + int sleep_time; // in ms + int reset_time; // in ms + bool video_widget; + + bool bEnableRoll; + bool bEnablePitch; + bool bEnableYaw; + bool bEnableX; + bool bEnableY; + bool bEnableZ; + + void load_ini(); + void save_ini() const; +}; + + +//----------------------------------------------------------------------------- +struct TrackerDialogSettings +{ + enum + { + MODEL_CLIP, + MODEL_CAP, + MODEL_CUSTOM + }; + int active_model_panel; + + int M01x; + int M01y; + int M01z; + int M02x; + int M02y; + int M02z; + int clip_ty; + int clip_tz; + int clip_by; + int clip_bz; + int cap_x; + int cap_y; + int cap_z; + + void load_ini(); + void save_ini() const; +}; + +#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 new file mode 100644 index 00000000..4aa1a658 --- /dev/null +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -0,0 +1,97 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "point_extractor.h" +#include + +using namespace cv; +using namespace std; + +// ---------------------------------------------------------------------------- +const vector& PointExtractor::extract_points(Mat frame, float dt, bool draw_output) +{ + /* + // sensitivity test for tracker + static int n = 0; + if (points.size() == 3) + { + for (int i=0; i<3; ++i) + { + points[i][0] -= 1e-4*sin(n/100.0); + points[i][1] -= 1e-4*cos(n/100.0); + } + ++n; + return points; + } + */ + + // convert to grayscale + Mat frame_bw; + cvtColor(frame, frame_bw, CV_RGB2GRAY); + + // convert to binary + threshold(frame_bw, frame_bw, threshold_val, 255, THRESH_BINARY); + //erode(frame_bw, frame_bw, Mat(), Point(-1,-1), min_size); //destroys information -> bad for subpixel accurarcy + + // find connected components... + // extract blobs with floodfill + struct BlobInfo + { + BlobInfo() : m00(0), m10(0), m01(0) {} + long m00; + long m10; + long m01; + }; + vector blobs; + int blob_count = 1; + + for (int y=0; y < frame_bw.rows; y++) { + for (int x=0; x < frame_bw.cols; x++) { + if (frame_bw.at(y,x) != 255) continue; + Rect rect; + floodFill(frame_bw, Point(x,y), Scalar(blob_count), &rect, Scalar(0), Scalar(0), 4); + BlobInfo blob; + for (int i=rect.y; i < (rect.y+rect.height); i++) { + for (int j=rect.x; j < (rect.x+rect.width); j++) { + if (frame_bw.at(i,j) != blob_count) continue; + blob.m00++; + blob.m01+=i; + blob.m10+=j; + } + } + blobs.push_back(blob); + blob_count++; + if (blob_count >= 255) break; + } + if (blob_count >= 255) break; + } + + // extract points + Vec2f c; + points.clear(); + float m00_min = 3.14*min_size*min_size; + float m00_max = 3.14*max_size*max_size; + for (vector::iterator iter = blobs.begin(); + iter!= blobs.end(); + ++iter) + { + const BlobInfo& m = *iter; + if (m.m00 < m00_min || m.m00 > m00_max) continue; + // convert to centered camera coordinate system with y axis upwards + c[0] = (m.m10/float(m.m00) - frame.cols/2)/frame.cols; + c[1] = -(m.m01/float(m.m00) - frame.rows/2)/frame.cols; + points.push_back(c); + } + + // draw output image + if (draw_output) + { + frame.setTo(Scalar(255,0,0), frame_bw); + } + + return points; +} diff --git a/ftnoir_tracker_pt/point_extractor.h b/ftnoir_tracker_pt/point_extractor.h new file mode 100644 index 00000000..b142d2bb --- /dev/null +++ b/ftnoir_tracker_pt/point_extractor.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef POINTEXTRACTOR_H +#define POINTEXTRACTOR_H + +#include + +// ---------------------------------------------------------------------------- +// Extracts points from an opencv image +class PointExtractor +{ +public: + // extracts points from frame and draws some processing info into frame, if draw_output is set + // dt: time since last call in seconds + // WARNING: returned reference is valid as long as object + const std::vector& extract_points(cv::Mat frame, float dt, bool draw_output); + const std::vector& get_points() { return points; } + + int threshold_val; + int min_size, max_size; + +protected: + std::vector points; +}; + +#endif //POINTEXTRACTOR_H diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp new file mode 100644 index 00000000..d617de19 --- /dev/null +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -0,0 +1,352 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "point_tracker.h" + +#include +#include +#include + +#include + +using namespace cv; +using namespace boost; +using namespace std; + +const float PI = 3.14159265358979323846f; + +// ---------------------------------------------------------------------------- +static void get_row(const Matx33f& m, int i, Vec3f& v) +{ + v[0] = m(i,0); + v[1] = m(i,1); + v[2] = m(i,2); +} + +static void set_row(Matx33f& m, int i, const Vec3f& v) +{ + m(i,0) = v[0]; + m(i,1) = v[1]; + m(i,2) = v[2]; +} + +// ---------------------------------------------------------------------------- +PointModel::PointModel(Vec3f M01, Vec3f M02) + : M01(M01), M02(M02) +{ + // calculate u + u = M01.cross(M02); + u /= norm(u); + + // calculate projection matrix on M01,M02 plane + float s11 = M01.dot(M01); + float s12 = M01.dot(M02); + float s22 = M02.dot(M02); + P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, + -s12, s11); + + // calculate d and d_order for simple freetrack-like point correspondence + vector points; + points.push_back(Vec2f(0,0)); + points.push_back(Vec2f(M01[0], M01[1])); + points.push_back(Vec2f(M02[0], M02[1])); + // fit line to orthographically projected points + // ERROR: yields wrong results with colinear points?! + /* + Vec4f line; + fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01); + d[0] = line[0]; d[1] = line[1]; + */ + // TODO: fix this + d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]); + + // sort model points + get_d_order(points, d_order); +} + +void PointModel::get_d_order(const std::vector& points, int d_order[]) const +{ + // get sort indices with respect to d scalar product + vector< pair > d_vals; + for (int i = 0; i(d.dot(points[i]), i)); + + struct + { + bool operator()(const pair& a, const pair& b) { return a.first < b.first; } + } comp; + sort(d_vals.begin(), d_vals.end(), comp); + + for (int i = 0; i& points, float f, float dt) +{ + if (!dynamic_pose_resolution) init_phase = true; + + dt_valid += dt; + // if there was no valid tracking result for too long, do a reset + if (dt_valid > dt_reset) + { + //qDebug()<<"dt_valid "< dt_reset "<& points, float f) +{ + if (init_phase) { + // We do a simple freetrack-like sorting in the init phase... + // sort points + int point_d_order[PointModel::N_POINTS]; + point_model->get_d_order(points, point_d_order); + + // set correspondences + for (int i=0; id_order[i]] = points[point_d_order[i]]; + } + } + else { + // ... otherwise we look at the distance to the projection of the expected model points + // project model points under current pose + p_exp[0] = project(Vec3f(0,0,0), f); + p_exp[1] = project(point_model->M01, f); + p_exp[2] = project(point_model->M02, f); + + // set correspondences by minimum distance to projected model point + bool point_taken[PointModel::N_POINTS]; + for (int i=0; iM01)/Z0; + epsilon_2 = k.dot(point_model->M02)/Z0; + + // vector of scalar products and + Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], + p[2][0]*(1.0 + epsilon_2) - p[0][0]); + Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1], + p[2][1]*(1.0 + epsilon_2) - p[0][1]); + + // construct projection of I, J onto M0i plane: I0 and J0 + I0_coeff = point_model->P * I0_M0i; + J0_coeff = point_model->P * J0_M0i; + I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02; + J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02; + + // calculate u component of I, J + float II0 = I0.dot(I0); + float IJ0 = I0.dot(J0); + float JJ0 = J0.dot(J0); + float rho, theta; + if (JJ0 == II0) { + rho = sqrt(abs(2*IJ0)); + theta = -PI/4; + if (IJ0<0) theta *= -1; + } + else { + rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 )); + theta = atan( -2*IJ0 / (JJ0-II0) ); + if (JJ0 - II0 < 0) theta += PI; + theta /= 2; + } + + // construct the two solutions + I_1 = I0 + rho*cos(theta)*point_model->u; + I_2 = I0 - rho*cos(theta)*point_model->u; + + J_1 = J0 + rho*sin(theta)*point_model->u; + J_2 = J0 - rho*sin(theta)*point_model->u; + + float norm_const = 1.0/norm(I_1); // all have the same norm + + // create rotation matrices + I_1 *= norm_const; J_1 *= norm_const; + I_2 *= norm_const; J_2 *= norm_const; + + set_row(R_1, 0, I_1); + set_row(R_1, 1, J_1); + set_row(R_1, 2, I_1.cross(J_1)); + + set_row(R_2, 0, I_2); + set_row(R_2, 1, J_2); + set_row(R_2, 2, I_2.cross(J_2)); + + // the single translation solution + Z0 = norm_const * f; + + // pick the rotation solution closer to the expected one + // in simple metric d(A,B) = || I - A * B^T || + float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t()); + float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t()); + + if (R_1_deviation < R_2_deviation) + R_current = &R_1; + else + R_current = &R_2; + + get_row(*R_current, 2, k); + + // check for convergence condition + if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) + break; + old_epsilon_1 = epsilon_1; + old_epsilon_2 = epsilon_2; + } + + // apply results + X_CM.R = *R_current; + X_CM.t[0] = p[0][0] * Z0/f; + X_CM.t[1] = p[0][1] * Z0/f; + X_CM.t[2] = Z0; + + return i; + + //Rodrigues(X_CM.R, r); + //qDebug()<<"iter: "< +#include +#include + +// ---------------------------------------------------------------------------- +// Afine frame trafo +class FrameTrafo +{ +public: + FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} + FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} + + cv::Matx33f R; + cv::Vec3f t; +}; + +inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) +{ + return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); +} + +inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) +{ + return X.R*v + X.t; +} + +// ---------------------------------------------------------------------------- +// Describes a 3-point model +// nomenclature as in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] +class PointModel +{ + friend class PointTracker; +public: + static const int N_POINTS = 3; + + PointModel(cv::Vec3f M01, cv::Vec3f M02); + + const cv::Vec3f& get_M01() const { return M01; }; + const cv::Vec3f& get_M02() const { return M02; }; + +protected: + cv::Vec3f M01; // M01 in model frame + cv::Vec3f M02; // M02 in model frame + + cv::Vec3f u; // unit vector perpendicular to M01,M02-plane + + cv::Matx22f P; + + cv::Vec2f d; // discrimant vector for point correspondence + int d_order[3]; // sorting of projected model points with respect to d scalar product + + void get_d_order(const std::vector& points, int d_order[]) const; +}; + +// ---------------------------------------------------------------------------- +// Tracks a 3-point model +// implementing the POSIT algorithm for coplanar points as presented in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] +class PointTracker +{ +public: + PointTracker(); + + // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) + // f : (focal length)/(sensor width) + // dt : time since last call + bool track(const std::vector& points, float f, float dt); + boost::shared_ptr point_model; + + bool dynamic_pose_resolution; + float dt_reset; + + FrameTrafo get_pose() const { return X_CM; } + void reset(); + +protected: + inline cv::Vec2f project(const cv::Vec3f& v_M, float f) + { + cv::Vec3f v_C = X_CM * v_M; + return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]); + } + + bool find_correspondences(const std::vector& points, float f); + + cv::Vec2f p[PointModel::N_POINTS]; // the points in model order + cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions + + void predict(float dt); + void update_velocities(float dt); + void reset_velocities(); + + + int POSIT(float f); // The POSIT algorithm, returns the number of iterations + + bool init_phase; + float dt_valid; // time since last valid tracking result + cv::Vec3f v_t; // velocities + cv::Vec3f v_r; + FrameTrafo X_CM; // trafo from model to camera + FrameTrafo X_CM_old; +}; + +#endif //POINTTRACKER_H diff --git a/ftnoir_tracker_pt/resource.h b/ftnoir_tracker_pt/resource.h new file mode 100644 index 00000000..c14e94ad --- /dev/null +++ b/ftnoir_tracker_pt/resource.h @@ -0,0 +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 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/trans_calib.cpp b/ftnoir_tracker_pt/trans_calib.cpp new file mode 100644 index 00000000..9b75a1b6 --- /dev/null +++ b/ftnoir_tracker_pt/trans_calib.cpp @@ -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. + */ + +#include "trans_calib.h" + +using namespace cv; + +//----------------------------------------------------------------------------- +TranslationCalibrator::TranslationCalibrator() +{ + reset(); +} + +void TranslationCalibrator::reset() +{ + P = Matx66f::zeros(); + y = Vec6f(0,0,0, 0,0,0); +} + +void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) +{ + Matx H_k_T = Matx::zeros(); + for (int i=0; i<3; ++i) { + for (int j=0; j<3; ++j) { + H_k_T(i,j) = R_CM_k(j,i); + } + } + for (int i=0; i<3; ++i) + { + H_k_T(3+i,i) = 1.0; + } + P += H_k_T * H_k_T.t(); + y += H_k_T * t_CM_k; +} + +Vec3f TranslationCalibrator::get_estimate() +{ + Vec6f x = P.inv() * y; + return Vec3f(-x[0], -x[1], -x[2]); +} \ No newline at end of file diff --git a/ftnoir_tracker_pt/trans_calib.h b/ftnoir_tracker_pt/trans_calib.h new file mode 100644 index 00000000..4024d011 --- /dev/null +++ b/ftnoir_tracker_pt/trans_calib.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef TRANSCALIB_H +#define TRANSCALIB_H + +#include + +//----------------------------------------------------------------------------- +// calibrates the translation from head to model = t_MH +// by recursive least squares / +// kalman filter in information form with identity noise covariance +// measurement equation when head position = t_CH is fixed: +// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k + +class TranslationCalibrator +{ +public: + TranslationCalibrator(); + + // reset the calibration process + void reset(); + + // update the current estimate + void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + + // get the current estimate for t_MH + cv::Vec3f get_estimate(); + +protected: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) +}; + +#endif //TRANSCALIB_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 diff --git a/ftnoir_tracker_pt/video_widget.cpp b/ftnoir_tracker_pt/video_widget.cpp new file mode 100644 index 00000000..c2b41da1 --- /dev/null +++ b/ftnoir_tracker_pt/video_widget.cpp @@ -0,0 +1,98 @@ +/* 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. + * + * 20130312, WVR: Add 7 lines to resizeGL after resize_frame. This should lower CPU-load. + */ + +#include "video_widget.h" + +#include + +using namespace cv; +using namespace std; +using namespace boost; + +// ---------------------------------------------------------------------------- +void VideoWidget::initializeGL() +{ + glClearColor(0.0, 0.0, 0.0, 0.0); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); +} + +void VideoWidget::resizeGL(int w, int h) +{ + // setup 1 to 1 projection + glViewport(0, 0, w, h); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glOrtho(0, w, 0, h, -1, 1); + resize_frame(); + glDisable(GL_DEPTH_TEST); + glBegin(GL_QUADS); + glVertex2f(0,0); + glVertex2f(1,0); + glVertex2f(1,1); + glVertex2f(0,1); + glEnd(); +} + +void VideoWidget::paintGL() +{ + glClear(GL_COLOR_BUFFER_BIT); + if (!resized_qframe.isNull()) + { + glDrawPixels(resized_qframe.width(), resized_qframe.height(), GL_RGBA, GL_UNSIGNED_BYTE, resized_qframe.bits()); + + const int crosshair_radius = 10; + const int crosshair_thickness = 1; + + glColor3f(1.0, 0.0, 0.0); + glLineWidth(crosshair_thickness); + int x,y; + for (vector::iterator iter = points->begin(); + iter != points->end(); + ++iter) + { + x = (*iter)[0] * resized_qframe.width() + resized_qframe.width()/2.0 + 0.5; + y = (*iter)[1] * resized_qframe.width() + resized_qframe.height()/2.0 + 0.5; + + glBegin(GL_LINES); + glVertex2i(x-crosshair_radius, y); + glVertex2i(x+crosshair_radius, y); + glEnd(); + glBegin(GL_LINES); + glVertex2i(x, y-crosshair_radius); + glVertex2i(x, y+crosshair_radius); + glEnd(); + } + } + glFlush(); +} + + +void VideoWidget::resize_frame() +{ + if (!qframe.isNull()) + resized_qframe = qframe.scaled(this->size(), Qt::KeepAspectRatio); +} + + +void VideoWidget::update(Mat frame, shared_ptr< vector > points) +{ + this->frame = frame; + this->points = points; + + // convert to QImage + if (frame.channels() == 3) + qframe = QImage((const unsigned char*)(frame.data), frame.cols, frame.rows, frame.step, QImage::Format_RGB888).rgbSwapped(); + else if (frame.channels() == 1) + qframe = QImage((const unsigned char*)(frame.data), frame.cols, frame.rows, frame.step, QImage::Format_Indexed8); + qframe = QGLWidget::convertToGLFormat(qframe); + + resize_frame(); + updateGL(); +} diff --git a/ftnoir_tracker_pt/video_widget.h b/ftnoir_tracker_pt/video_widget.h new file mode 100644 index 00000000..f49fef18 --- /dev/null +++ b/ftnoir_tracker_pt/video_widget.h @@ -0,0 +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. + */ + +#ifndef VIDEOWIDGET_H +#define VIDEOWIDGET_H + +#include +#include +#include +#include + +// ---------------------------------------------------------------------------- +class VideoWidget : public QGLWidget +{ + Q_OBJECT + +public: + VideoWidget(QWidget *parent) : QGLWidget(parent) {} + + void initializeGL(); + void resizeGL(int w, int h); + void paintGL(); + + void update(cv::Mat frame, boost::shared_ptr< std::vector > points); + +private: + void resize_frame(); + + cv::Mat frame; + QImage qframe; + QImage resized_qframe; + + boost::shared_ptr< std::vector > points; +}; + +#endif // VIDEOWIDGET_H -- cgit v1.2.3