From cb233e361a859bc3271cdccd701e1690af459592 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 1 Dec 2013 12:17:43 +0100 Subject: accela: implement a third-order highpass filter Signed-off-by: Stanislaw Halik --- .../ftnoir_accela_filtercontrols.ui | 88 +++++++++++++++++++--- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 16 +++- ftnoir_filter_accela/ftnoir_filter_accela.h | 7 +- .../ftnoir_filter_accela_dialog.cpp | 4 + ftnoir_filter_kalman/ftnoir_filter_kalman.h | 33 ++++---- ftnoir_filter_kalman/kalman.cpp | 60 ++++++++++----- 6 files changed, 166 insertions(+), 42 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui index 5ba33614..b4b12061 100644 --- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui +++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui @@ -10,7 +10,7 @@ 0 0 347 - 268 + 339 @@ -109,7 +109,7 @@ QAbstractSpinBox::CorrectToPreviousValue - 3 + 4 0.100000000000000 @@ -147,7 +147,7 @@ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - 3 + 4 0.100000000000000 @@ -160,7 +160,7 @@ - + @@ -189,7 +189,7 @@ background:none; - + @@ -229,7 +229,7 @@ background:none; - + @@ -258,7 +258,7 @@ background:none; - + @@ -286,14 +286,14 @@ background:none; - + Exponent - + @@ -315,6 +315,76 @@ background:none; + + + + Order #2 + + + + + + + + 0 + 0 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + QAbstractSpinBox::CorrectToPreviousValue + + + 4 + + + 0.100000000000000 + + + 65535.000000000000000 + + + 1.000000000000000 + + + + + + + + 0 + 0 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + QAbstractSpinBox::CorrectToPreviousValue + + + 4 + + + 0.100000000000000 + + + 65535.000000000000000 + + + 1.000000000000000 + + + + + + + Order #3 + + + diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 9189ec8a..1f9b493f 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -34,7 +34,8 @@ void FTNoIR_Filter::loadSettings() { zoom_factor = iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toDouble(); rotation_alpha = iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble(); translation_alpha = iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble(); - + second_order_alpha = iniFile.value("second-order-alpha", ACCELA_SECOND_ORDER_ALPHA).toDouble(); + third_order_alpha = iniFile.value("third-order-alpha", ACCELA_THIRD_ORDER_ALPHA).toDouble(); deadzone = iniFile.value("deadzone", 0.0).toDouble(); // bigger means less filtering static const double init_scaling[] = { @@ -81,6 +82,8 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, { new_camera_position[i] = target_camera_position[i]; current_camera_position[i] = target_camera_position[i]; + current_camera_position_2[i] = target_camera_position[i]; + current_camera_position_3[i] = target_camera_position[i]; } first_run = false; @@ -95,10 +98,19 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, const int sign = vec < 0 ? -1 : 1; const double x = fabs(vec); const double a = i >= 3 ? rotation_alpha : translation_alpha; + const double a2 = a * second_order_alpha; + const double a3 = a * third_order_alpha; + const double x2 = fabs(target_camera_position[i] - current_camera_position_2[i]); + const double x3 = fabs(target_camera_position[i] - current_camera_position_3[i]); const double reduction = 1. / std::max(1., 1. + zoom_factor * -last_post_filter_values[TZ] / 1000); - const double velocity = parabola(a, x * scaling[i], deadzone, expt) * reduction; + const double velocity = + parabola(a, x * scaling[i], deadzone, expt) * reduction + + parabola(a2, x2 * scaling[i], deadzone, expt) * reduction + + parabola(a3, x3 * scaling[i], deadzone, expt) * reduction; const double result = current_camera_position[i] + velocity * sign; const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i]; + current_camera_position_3[i] = current_camera_position_2[i]; + current_camera_position_2[i] = current_camera_position[i]; new_camera_position[i] = current_camera_position[i] = done ? target_camera_position[i] : result; } } diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index 8c770df9..eec97758 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -33,7 +33,9 @@ #define ACCELA_SMOOTHING_ROTATION 60.0 #define ACCELA_SMOOTHING_TRANSLATION 40.0 -#define ACCELA_ZOOM_SLOWNESS 35 +#define ACCELA_ZOOM_SLOWNESS 0 +#define ACCELA_SECOND_ORDER_ALPHA 100.0 +#define ACCELA_THIRD_ORDER_ALPHA 180.0 //******************************************************************************************************* // FaceTrackNoIR Filter class. @@ -53,7 +55,10 @@ private: void loadSettings(); bool first_run; double rotation_alpha, translation_alpha, zoom_factor; + double second_order_alpha, third_order_alpha; double current_camera_position[6]; + double current_camera_position_2[6]; + double current_camera_position_3[6]; double scaling[6]; double deadzone; double expt; diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp index 88c0a794..68484e77 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp @@ -160,6 +160,8 @@ void FilterControls::loadSettings() { ui.spinZoom->setValue(iniFile.value("zoom-slowness", ACCELA_ZOOM_SLOWNESS).toInt()); ui.rotation_alpha->setValue(iniFile.value("rotation-alpha", ACCELA_SMOOTHING_ROTATION).toDouble()); ui.translation_alpha->setValue(iniFile.value("translation-alpha", ACCELA_SMOOTHING_TRANSLATION).toDouble()); + ui.order_2nd->setValue(iniFile.value("second-order-alpha", ACCELA_SECOND_ORDER_ALPHA).toDouble()); + ui.order_3rd->setValue(iniFile.value("third-order-alpha", ACCELA_THIRD_ORDER_ALPHA).toDouble()); ui.deadzone->setValue(iniFile.value("deadzone", 0).toDouble()); // bigger means less filtering @@ -212,6 +214,8 @@ void FilterControls::save() { iniFile.setValue("zoom-slowness", zoom = ui.spinZoom->value()); iniFile.setValue("deadzone", deadzone = ui.deadzone->value()); iniFile.setValue("exponent", expt = ui.expt->value()); + iniFile.setValue("second-order-alpha", rot = ui.order_2nd->value()); + iniFile.setValue("third-order-alpha", rot = ui.order_3rd->value()); QDoubleSpinBox* boxen[] = { ui.doubleSpinBox, diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h index 59169612..6c2cb6a9 100644 --- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -19,25 +19,29 @@ #include #include #include +#include #include class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter { public: FTNoIR_Filter(); - virtual ~FTNoIR_Filter() { + ~FTNoIR_Filter() virt_override { } - void Initialize(); - void FilterHeadPoseData(double *current_camera_position, - double *target_camera_position, + void Initialize() virt_override; + void FilterHeadPoseData(const double *target_camera_position, double *new_camera_position, - double *last_post_filter_values); + const double *) virt_override; cv::KalmanFilter kalman; double prev_position[6]; + double prev2_filter_pos[6]; + double prev_filter_pos[6]; + QElapsedTimer timer; + qint64 timedelta; }; -void kalman_load_settings(FTNoIR_Filter& self); -void kalman_save_settings(FTNoIR_Filter& self); +void kalman_load_settings(FTNoIR_Filter&); +void kalman_save_settings(FTNoIR_Filter&); class FTNOIR_FILTER_BASE_EXPORT FTNoIR_FilterDll : public Metadata { @@ -48,7 +52,7 @@ public: void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); } }; -class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, Ui::KalmanUICFilterControls, public IFilterDialog +class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, public IFilterDialog { Q_OBJECT public: @@ -65,21 +69,24 @@ public: connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel())); show(); } - virtual ~FilterControls() {} - void showEvent ( QShowEvent * event ) { + ~FilterControls() {} + void showEvent ( QShowEvent * ) virt_override { show(); } - void Initialize(QWidget *parent, IFilter* ptr) { + void Initialize(QWidget *) virt_override { + show(); + raise(); } bool settingsDirty; Ui::KalmanUICFilterControls ui; - + virtual void registerFilter(IFilter*) virt_override {} + virtual void unregisterFilter() virt_override {} public slots: void doOK(); void doCancel(); - void settingsChanged(double unused) { + void settingsChanged(double) { settingsDirty = true; } }; diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp index 51af35e1..5ecd417c 100644 --- a/ftnoir_filter_kalman/kalman.cpp +++ b/ftnoir_filter_kalman/kalman.cpp @@ -9,7 +9,7 @@ #include #include -void kalman_load_settings(FTNoIR_Filter& self) { +void kalman_load_settings(FTNoIR_Filter&) { QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); @@ -19,7 +19,7 @@ void kalman_load_settings(FTNoIR_Filter& self) { iniFile.endGroup(); } -void kalman_save_settings(FilterControls& self) { +void kalman_save_settings(FilterControls&) { QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); @@ -37,8 +37,8 @@ FTNoIR_Filter::FTNoIR_Filter() { // the following was written by Donovan Baarda // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b void FTNoIR_Filter::Initialize() { - const double accel_variance = 1e-4; - const double noise_variance = 1e1; + const double accel_variance = 1e-3; + const double noise_variance = 5e2; kalman.init(12, 6, 0, CV_64F); kalman.transitionMatrix = (cv::Mat_(12, 12) << 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, @@ -73,13 +73,28 @@ void FTNoIR_Filter::Initialize() { cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance)); cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4)); for (int i = 0; i < 6; i++) + { prev_position[i] = 0; + prev2_filter_pos[i] = 0; + prev_filter_pos[i] = 0; + timedelta = 1; + timer.invalidate(); + } +} + +template +static inline T clamp(const T min, const T max, const T value) +{ + if (value < min) + return min; + if (value > max) + return max; + return value; } -void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, - double *target_camera_position, +void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, double *new_camera_position, - double *last_post_filter_values) + const double *) { bool new_target = false; @@ -89,23 +104,34 @@ void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, new_target = true; break; } - - cv::Mat output = kalman.predict(); - + if (new_target) { + cv::Mat output = kalman.predict(); cv::Mat measurement(6, 1, CV_64F); for (int i = 0; i < 3; i++) { measurement.at(i) = target_camera_position[i+3]; measurement.at(i+3) = target_camera_position[i]; } kalman.correct(measurement); - } - - for (int i = 0; i < 3; i++) { - new_camera_position[i] = output.at(i+3); - new_camera_position[i+3] = output.at(i); - prev_position[i] = target_camera_position[i]; - prev_position[i+3] = target_camera_position[i+3]; + for (int i = 0; i < 6; i++) + { + prev_position[i] = target_camera_position[i]; + } + if (timer.isValid()) + timedelta = timer.elapsed(); + else + timedelta = 1; + for (int i = 0; i < 6; i++) + { + prev2_filter_pos[i] = prev_filter_pos[i]; + prev_filter_pos[i] = new_camera_position[i] = output.at((i + 3) % 6); + } + timer.start(); + } else { + auto d = timer.isValid() ? timer.elapsed() : 1; + auto c = clamp(0.0, 1.0, d / (double) timedelta); + for (int i = 0; i < 6; i++) + new_camera_position[i] = prev2_filter_pos[i] + (prev_filter_pos[i] - prev2_filter_pos[i]) * c; } } -- cgit v1.2.3