From 20db817b7805504165f035870036e05e1b7edb23 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Sat, 3 Aug 2013 22:53:39 +1000 Subject: Tidy and optimize ewma2 filter. --- ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 204 ++++++++++++---------------- ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 64 +++++---- 2 files changed, 118 insertions(+), 150 deletions(-) diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index 3de1794a..67a15afe 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -38,142 +38,112 @@ FTNoIR_Filter::FTNoIR_Filter() { - first_run = true; - alpha_smoothing = 0.02f; // this is a constant for now, might be a parameter later - loadSettings(); // Load the Settings - + first_run = true; + alpha_smoothing = 0.02f; // this is a constant for now, might be a parameter later + loadSettings(); // Load the Settings } FTNoIR_Filter::~FTNoIR_Filter() { - } // // Load the current Settings from the currently 'active' INI-file. // void FTNoIR_Filter::loadSettings() { - qDebug() << "FTNoIR_Filter::loadSettings says: Starting "; - QSettings settings("opentrack"); // Registry settings (in HK_USER) - - QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) - - qDebug() << "FTNoIR_Filter::loadSettings says: iniFile = " << currentFile; - - // - // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR - // - iniFile.beginGroup ( "Tracking" ); - kMinSmoothing = iniFile.value ( "minSmooth", 15 ).toInt(); - kMaxSmoothing = iniFile.value ( "maxSmooth", 50 ).toInt(); - kSmoothingScaleCurve = iniFile.value ( "powCurve", 10 ).toInt(); - iniFile.endGroup (); - + qDebug() << "FTNoIR_Filter::loadSettings says: Starting "; + QSettings settings("opentrack"); // Registry settings (in HK_USER) + + QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); + QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file) + + qDebug() << "FTNoIR_Filter::loadSettings says: iniFile = " << currentFile; + + // + // The EWMA2-filter-settings are in the Tracking group: this is because they used to be on the Main Form of FaceTrackNoIR + // + iniFile.beginGroup ( "Tracking" ); + kMinSmoothing = iniFile.value ( "minSmooth", 15 ).toInt(); + kMaxSmoothing = iniFile.value ( "maxSmooth", 50 ).toInt(); + kSmoothingScaleCurve = iniFile.value ( "powCurve", 10 ).toInt(); + iniFile.endGroup (); } -void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter) +void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, + double *target_camera_position, + double *new_camera_position, + double *last_post_filter) { - //non-optimised version for clarity - double prev_output[6]; - double target[6]; - double output_delta[6]; + double delta; + double new_alpha; double scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f}; - double norm_output_delta[6]; - double output[6]; - for (int i = 0; i < 6; i++) - { - prev_output[i] = current_camera_position[i]; - target[i] = target_camera_position[i]; + //On the first run, initialize to output=target and return. + if (first_run==true) { + for (int i=0;i<6;i++) { + new_camera_position[i] = target_camera_position[i]; + current_camera_position[i] = target_camera_position[i]; + alpha[i] = 0.0f; + } + first_run=false; + return; } - if (first_run==true) - { - //on the first run, output=target - for (int i=0;i<6;i++) - { - output[i]=target[i]; - prev_alpha[i] = 0.0f; - } - - for (int i = 0; i < 6; i++) - new_camera_position[i] = target[i]; - - first_run=false; - - //we can bail - return; - } - - //how far does the camera need to move to catch up? - for (int i=0;i<6;i++) - { - output_delta[i]=(target[i]-prev_output[i]); - } - - //normalise the deltas - for (int i=0;i<6;i++) - { - norm_output_delta[i]=std::min(std::max(fabs(output_delta[i])/scale[i],0.0),1.0); - } - - //calculate the alphas - //work out the dynamic smoothing factors -// if (newTarget) { - for (int i=0;i<6;i++) - { - alpha[i]=1.0/(kMinSmoothing+((1.0-pow(norm_output_delta[i],kSmoothingScaleCurve))*(kMaxSmoothing - kMinSmoothing))); - smoothed_alpha[i]=(alpha_smoothing*alpha[i])+((1.0f-alpha_smoothing)*prev_alpha[i]); - } -// } - - //qDebug() << "FTNoIR_Filter::FilterHeadPoseData() smoothing frames = " << smoothing_frames_range; - //qDebug() << "FTNoIR_Filter::FilterHeadPoseData() alpha[3] = " << alpha[3]; - - //use the same (largest) smoothed alpha for each channel - //NB: larger alpha = *less* lag (opposite to what you'd expect) - float largest_alpha=0.0f; - for (int i=0;i<6;i++) - { - if (smoothed_alpha[i]>=largest_alpha) - { - largest_alpha=smoothed_alpha[i]; - } - } - - //move the camera - for (int i=0;i<6;i++) - { - output[i]=(largest_alpha*target[i])+((1.0f-largest_alpha)*prev_output[i]); -// output[i]=(smoothed_alpha[i]*target[i])+((1.0f-smoothed_alpha[i])*prev_output[i]); - } - - - #ifdef LOG_OUTPUT - // Use this for some debug-output to file... - QFile data(QCoreApplication::applicationDirPath() + "\\EWMA_output.txt"); - if (data.open(QFile::WriteOnly | QFile::Append)) { - QTextStream out(&data); - out << "output:\t" << output[0] << "\t" << output[1] << "\t" << output[2] << "\t" << output[3] << "\t" << output[4] << "\t" << output[5] << '\n'; - out << "target:\t" << target[0] << "\t" << target[1] << "\t" << target[2] << "\t" << target[3] << "\t" << target[4] << "\t" << target[5] << '\n'; - out << "prev_output:\t" << prev_output[0] << "\t" << prev_output[1] << "\t" << prev_output[2] << "\t" << prev_output[3] << "\t" << prev_output[4] << "\t" << prev_output[5] << '\n'; - out << "largest_alpha:\t" << largest_alpha << '\n'; - } - #endif - - for (int i = 0; i < 6; i++) - { - new_camera_position[i] = output[i]; - current_camera_position[i] = output[i]; + for (int i=0;i<6;i++) { + // Calculate the delta. + delta=target_camera_position[i]-current_camera_position[i]; + // Normalise the delta. + delta=std::min(std::max(fabs(delta)/scale[i],0.0),1.0); + // Calculate the new alpha from the normalized delta. + new_alpha=1.0/(kMinSmoothing+((1.0-pow(delta,kSmoothingScaleCurve))*(kMaxSmoothing-kMinSmoothing))); + // Update the smoothed alpha. + alpha[i]=(alpha_smoothing*new_alpha)+((1.0f-alpha_smoothing)*alpha[i]); } - //update filter memories ready for next sample - for (int i=0;i<6;i++) - { - prev_alpha[i]=smoothed_alpha[i]; - } - return; + // Use the same (largest) smoothed alpha for each channel + //NB: larger alpha = *less* lag (opposite to what you'd expect) + float largest_alpha=0.0f; + for (int i=0;i<6;i++) { + largest_alpha=std::max(largest_alpha, alpha[i]); + } + + // Calculate the new camera position. + for (int i=0;i<6;i++) { + new_camera_position[i]=(largest_alpha*target_camera_position[i])+((1.0f-largest_alpha)*current_camera_position[i]); + //new_camera_position[i]=(alpha[i]*target_camera_position[i])+((1.0f-alpha[i])*current_camera_position[i]); + } + +#ifdef LOG_OUTPUT + // Use this for some debug-output to file... + QFile data(QCoreApplication::applicationDirPath() + "\\EWMA_output.txt"); + if (data.open(QFile::WriteOnly | QFile::Append)) { + QTextStream out(&data); + out << "current:\t" << current_camera_position[0] + << "\t" << current_camera_position[1] + << "\t" << current_camera_position[2] + << "\t" << current_camera_position[3] + << "\t" << current_camera_position[4] + << "\t" << current_camera_position[5] << '\n'; + out << "target:\t" << target_camera_position[0] + << "\t" << target_camera_position[1] + << "\t" << target_camera_position[2] + << "\t" << target_camera_position[3] + << "\t" << target_camera_position[4] + << "\t" << target_camera_position[5] << '\n'; + out << "output:\t" << new_camera_position[0] + << "\t" << new_camera_position[1] + << "\t" << new_camera_position[2] + << "\t" << new_camera_position[3] + << "\t" << new_camera_position[4] + << "\t" << new_camera_position[5] << '\n'; + out << "largest_alpha:\t" << largest_alpha << '\n'; + } +#endif + + // Update the current camera position to the new position. + for (int i = 0; i < 6; i++) { + current_camera_position[i] = new_camera_position[i]; + } } //////////////////////////////////////////////////////////////////////////////// diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index 28824d2d..b4cb1d64 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -39,25 +39,26 @@ class FTNoIR_Filter : public IFilter { public: - FTNoIR_Filter(); + FTNoIR_Filter(); ~FTNoIR_Filter(); void Initialize() {} - void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter); + void FilterHeadPoseData(double *current_camera_position, + double *target_camera_position, + double *new_camera_position, + double *last_post_filter); private: - void loadSettings(); // Load the settings from the INI-file - double newHeadPose; // Structure with new headpose + void loadSettings(); // Load the settings from the INI-file + double newHeadPose; // Structure with new headpose - bool first_run; - double alpha_smoothing; - double prev_alpha[6]; - double alpha[6]; - double smoothed_alpha[6]; + bool first_run; + double alpha_smoothing; + double alpha[6]; - double kMinSmoothing; - double kMaxSmoothing; - double kSmoothingScaleCurve; + double kMinSmoothing; + double kMaxSmoothing; + double kSmoothingScaleCurve; }; //******************************************************************************************************* @@ -69,27 +70,26 @@ class FilterControls: public QWidget, public IFilterDialog { Q_OBJECT public: - - explicit FilterControls(); + explicit FilterControls(); virtual ~FilterControls(); - void showEvent ( QShowEvent * event ); + void showEvent ( QShowEvent * event ); void Initialize(QWidget *parent, IFilter* ptr); private: - Ui::UICFilterControls ui; - void loadSettings(); - void save(); + Ui::UICFilterControls ui; + void loadSettings(); + void save(); - /** helper **/ - bool settingsDirty; + /** helper **/ + bool settingsDirty; - IFilter* pFilter; // If the filter was active when the dialog was opened, this will hold a pointer to the Filter instance + IFilter* pFilter; // If the filter was active when the dialog was opened, this will hold a pointer to the Filter instance private slots: - void doOK(); - void doCancel(); - void settingChanged() { settingsDirty = true; }; - void settingChanged( int ) { settingsDirty = true; }; + void doOK(); + void doCancel(); + void settingChanged() { settingsDirty = true; }; + void settingChanged( int ) { settingsDirty = true; }; }; //******************************************************************************************************* @@ -98,15 +98,13 @@ private slots: class FTNoIR_FilterDll : public Metadata { public: - FTNoIR_FilterDll(); - ~FTNoIR_FilterDll(); - void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA Filter Mk2"); } - void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA"); } - void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Exponentially Weighted Moving Average filter with dynamic smoothing parameter"); } - - void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); } + FTNoIR_FilterDll(); + ~FTNoIR_FilterDll(); + void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA Filter Mk2"); } + void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("EWMA"); } + void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Exponentially Weighted Moving Average filter with dynamic smoothing parameter"); } + void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); } }; #endif //INCLUDED_FTN_FILTER_H //END - -- cgit v1.2.3