From 5fd06c5d0a28a861e02f000d95cf82d987465041 Mon Sep 17 00:00:00 2001 From: Patrick Ruoff Date: Sat, 12 Jan 2013 14:07:45 +0000 Subject: PointTracker: Tracker state is reported to FTNoIR, Camera pitch correction, Dynamic pose resolution made optional git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@207 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb --- FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui | 833 ++++++++++++++--------- FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp | 22 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.h | 5 + FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp | 22 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h | 2 + FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp | 36 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h | 16 +- FTNoIR_Tracker_PT/point_tracker.cpp | 4 +- FTNoIR_Tracker_PT/point_tracker.h | 2 + 9 files changed, 597 insertions(+), 345 deletions(-) (limited to 'FTNoIR_Tracker_PT') diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui index 20e16985..a2389f71 100644 --- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui +++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui @@ -9,8 +9,8 @@ 0 0 - 393 - 585 + 405 + 489 @@ -64,85 +64,57 @@ - TrackerThread + Tracker Thread - - - - - QFormLayout::AllNonFixedFieldsGrow + + + + + Whether to update the content of the VideoWidget - - - - Sleep time + + Show VideoWidget + + + + + + + Sleep time + + + sleep_spin + + + + + + + + + Time the tracker thread sleeps after each processed frame - - sleep_spin + + + + + 9999 - - - - - - Time the tracker thread sleeps after each processed frame - - - - - - 9999 - - - - - - - ms - - - - - - - + + - Auto-reset time - - - reset_spin + ms - - - - - - Time until automatic reset of tracker's internal state when no valid tracking result is found - - - 9999 - - - - - - - ms - - - - - - - + + Qt::Horizontal @@ -154,32 +126,420 @@ - - - - QFormLayout::AllNonFixedFieldsGrow + + + + Auto-reset time + + + reset_spin - - + + + + + + - Whether to update the content of the VideoWidget + Time until automatic reset of tracker's internal state when no valid tracking result is found + + + 9999 + + + + - Show VideoWidget + ms + + + + + + + Dynamic Pose Resolution + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + false + + + Center the tracker + + + Center + + + + + + + false + + + Reset the tracker's internal state + + + Reset + + + + + + + + + + Camera + + - Camera + Camera Settings @@ -325,6 +685,53 @@ + + + + + + Camera Pitch (upwards = positive) + + + campitch_spin + + + + + + + Qt::DefaultContextMenu + + + The angle the camera is facing upwards + + + -99 + + + + + + + deg + + + + + + + Qt::Horizontal + + + + 20 + 20 + + + + + + @@ -433,196 +840,14 @@ - - - - 0 - 85 - - - - Enable Axis - - - - - 10 - 20 - 143 - 60 - - - - - - - Roll: - - - - - - - Pitch: - - - - - - - Yaw: - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - X: - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - Y: - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - Z: - - - - - - - - 20 - 16777215 - - - - Qt::LeftToRight - - - - - - - - - - Qt::Horizontal - - - QSizePolicy::Minimum - - - - 40 - 20 - - - - - - - - - - + Qt::Vertical 20 - 20 + 40 @@ -689,36 +914,6 @@ - - - - - - false - - - Center the tracker - - - Center - - - - - - - false - - - Reset the tracker's internal state - - - Reset - - - - - @@ -732,7 +927,7 @@ QTabWidget::Rounded - 0 + 2 false @@ -1343,7 +1538,7 @@ - + @@ -1356,28 +1551,28 @@ - + x: - + y: - + z: - + -999 @@ -1387,7 +1582,7 @@ - + -999 @@ -1397,13 +1592,6 @@ - - - - T: - - - @@ -1526,21 +1714,27 @@ tabWidget - ok_button - cancel_button sleep_spin - reset_spin videowidget_check + reset_spin + dynpose_check + chkEnableRoll + chkEnablePitch + chkEnableYaw + chkEnableX + chkEnableY + chkEnableZ + center_button + reset_button camindex_spin res_x_spin res_y_spin fps_spin f_dspin + campitch_spin threshold_slider mindiam_spin maxdiam_spin - center_button - reset_button model_tabs clip_tlength_spin clip_theight_spin @@ -1559,11 +1753,30 @@ ty_spin tz_spin tcalib_button + ok_button + cancel_button - + + + dynpose_check + toggled(bool) + reset_spin + setEnabled(bool) + + + 285 + 105 + + + 139 + 111 + + + + startEngineClicked() stopEngineClicked() diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp index 412d35d4..e9394eff 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp @@ -20,7 +20,7 @@ using namespace boost; //----------------------------------------------------------------------------- Tracker::Tracker() - : frame_count(0), commands(0), video_widget(NULL) + : frame_count(0), commands(0), video_widget(NULL), tracking_valid(false) { qDebug()<<"Tracker::Tracker"; TrackerSettings settings; @@ -79,7 +79,7 @@ void Tracker::run() if (new_frame && !frame.empty()) { const std::vector& points = point_extractor.extract_points(frame, dt, draw_frame); - point_tracker.track(points, camera.get_info().f, dt); + tracking_valid = point_tracker.track(points, camera.get_info().f, dt); frame_count++; } #ifdef PT_PERF_LOG @@ -106,9 +106,11 @@ void Tracker::apply(const TrackerSettings& settings) point_extractor.min_size = settings.min_point_size; point_extractor.max_size = settings.max_point_size; point_tracker.point_model = boost::shared_ptr(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; @@ -188,9 +190,12 @@ void Tracker::StopTracker(bool exit) 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; @@ -202,13 +207,18 @@ bool Tracker::GiveHeadPoseData(THeadPoseData *data) 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; } + + // 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(); // translate rotation matrix from opengl (G) to roll-pitch-yaw (R) frame // -z -> x, y -> z, x -> -y @@ -225,13 +235,13 @@ bool Tracker::GiveHeadPoseData(THeadPoseData *data) gamma = atan2( R(2,1), R(2,2)); if (bEnableYaw) { - data->yaw = rad2deg * alpha; + data->yaw = rad2deg * alpha; } if (bEnablePitch) { - data->pitch = -rad2deg * beta; // this is what ftnoir expects? + data->pitch = rad2deg * beta; } if (bEnableRoll) { - data->roll = rad2deg * gamma; + data->roll = rad2deg * gamma; } } return true; diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h index c381b807..2533a39b 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h @@ -62,9 +62,14 @@ protected: 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; diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp index f3daae0c..83c04305 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp @@ -24,6 +24,7 @@ TrackerDialog::TrackerDialog() // 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); @@ -31,6 +32,7 @@ TrackerDialog::TrackerDialog() 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); @@ -62,6 +64,7 @@ TrackerDialog::TrackerDialog() // 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)) ); @@ -69,6 +72,7 @@ TrackerDialog::TrackerDialog() 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)) ); @@ -259,28 +263,36 @@ void TrackerDialog::doCancel() void TrackerDialog::poll_tracker_info() { if (tracker) - { + { + QString to_print; + // display caminfo CamInfo info; tracker->get_cam_info(&info); - ui.caminfo_label->setText(QString::number(info.res_x)+"x"+QString::number(info.res_y)+" @ "+QString::number(info.fps)+" FPS"); + 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(); - QString to_print = QString::number(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 { - ui.caminfo_label->setText("Tracker offline"); - ui.pointinfo_label->setText("Tracker offline"); + 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); } } diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h index 19404444..0f836dfe 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h @@ -34,6 +34,7 @@ public: 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(); } @@ -41,6 +42,7 @@ protected slots: 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(); } diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp index 7c356694..40d1bc4f 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.cpp @@ -25,6 +25,7 @@ void TrackerSettings::load_ini() 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(); @@ -37,16 +38,17 @@ void TrackerSettings::load_ini() 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(); + 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(); } @@ -62,10 +64,11 @@ void TrackerSettings::save_ini() const 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("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); @@ -78,16 +81,17 @@ void TrackerSettings::save_ini() const 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.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(); } diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h index 16ee20f3..88162c86 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h @@ -21,6 +21,7 @@ struct TrackerSettings int cam_res_x; int cam_res_y; int cam_fps; + int cam_pitch; // point extraction int threshold; @@ -30,13 +31,7 @@ struct TrackerSettings // point tracking cv::Vec3f M01; cv::Vec3f M02; - - bool bEnableRoll; - bool bEnablePitch; - bool bEnableYaw; - bool bEnableX; - bool bEnableY; - bool bEnableZ; + bool dyn_pose_res; // head to model translation cv::Vec3f t_MH; @@ -45,6 +40,13 @@ struct TrackerSettings 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; }; diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index 03914bb4..d617de19 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -87,7 +87,7 @@ void PointModel::get_d_order(const std::vector& points, int d_order[] // ---------------------------------------------------------------------------- -PointTracker::PointTracker() : init_phase(true), dt_valid(0), dt_reset(1), v_t(0,0,0), v_r(0,0,0) +PointTracker::PointTracker() : init_phase(true), dt_valid(0), dt_reset(1), v_t(0,0,0), v_r(0,0,0), dynamic_pose_resolution(true) { X_CM.t[2] = 1000; // default position: 1 m away from cam; } @@ -109,6 +109,8 @@ void PointTracker::reset_velocities() bool PointTracker::track(const vector& points, float f, float dt) { + if (!dynamic_pose_resolution) init_phase = true; + dt_valid += dt; // if there was no valid tracking result for too long, do a reset if (dt_valid > dt_reset) diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index efd40424..21baad19 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -74,9 +74,11 @@ public: // 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; } -- cgit v1.2.3