From cf80e5b7471cfd5506319aa32e697c9688b1b4be Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 2 Apr 2013 18:31:34 +0200 Subject: merge continues --- ftnoir_filter_kalman/ftnoir_filter_kalman.h | 94 +++++++++ .../ftnoir_kalman_filtercontrols.ui | 211 +++++++++++++++++++++ ftnoir_filter_kalman/images/filter-16-ac.png | Bin 0 -> 725 bytes ftnoir_filter_kalman/kalman-filter.qrc | 5 + ftnoir_filter_kalman/kalman.cpp | 153 +++++++++++++++ 5 files changed, 463 insertions(+) create mode 100644 ftnoir_filter_kalman/ftnoir_filter_kalman.h create mode 100644 ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui create mode 100644 ftnoir_filter_kalman/images/filter-16-ac.png create mode 100644 ftnoir_filter_kalman/kalman-filter.qrc create mode 100644 ftnoir_filter_kalman/kalman.cpp (limited to 'ftnoir_filter_kalman') diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h new file mode 100644 index 00000000..15ccc8c5 --- /dev/null +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -0,0 +1,94 @@ +#pragma once +/* Copyright (c) 2013 Stanisław Halik + * + * 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 INCLUDED_FTN_FILTER_H +#define INCLUDED_FTN_FILTER_H + +#undef FTNOIR_TRACKER_BASE_LIB +#define FTNOIR_TRACKER_BASE_EXPORT Q_DECL_IMPORT + +#include "ftnoir_filter_base/ftnoir_filter_base.h" +#include "ui_ftnoir_kalman_filtercontrols.h" +#include "facetracknoir/global-settings.h" +#include +#include +#include +#include +#include +#include + +#define DEFAULT_ACCL 750 + +class FTNOIR_FILTER_BASE_EXPORT FTNoIR_Filter : public IFilter +{ +public: + FTNoIR_Filter(); + virtual ~FTNoIR_Filter() { + } + void Initialize(); + void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget); + cv::KalmanFilter kalman; + double process_noise_covariance_matrix_all_values; + double posteriori_error_covariance_matrix_all_values; + double accl; +}; + +void kalman_load_settings(FTNoIR_Filter& self); +void kalman_save_settings(FTNoIR_Filter& self); + +class FTNOIR_FILTER_BASE_EXPORT FTNoIR_FilterDll : public Metadata +{ +public: + void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getShortName(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getDescription(QString *strToBeFilled) { *strToBeFilled = QString("Kalman filter"); } + void getIcon(QIcon *icon){ *icon = QIcon(":/images/filter-16.png"); } +}; + +class FTNOIR_FILTER_BASE_EXPORT FilterControls: public QWidget, Ui::KalmanUICFilterControls, public IFilterDialog +{ + Q_OBJECT +public: + explicit FilterControls() : settingsDirty(false) { + ui.setupUi(this); + 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("ftnoir-filter-kalman"); + ui.post->setValue(iniFile.value("posteriori-error-covariance-matrix-all-values", "1e-12").toDouble()); + ui.pnoise->setValue(iniFile.value("process-noise-covariance-matrix-all-values", "1e-14").toDouble()); + ui.accl->setValue(iniFile.value("accel-coefficient", DEFAULT_ACCL).toDouble()); + iniFile.endGroup(); + connect(ui.btnOk, SIGNAL(clicked()), this, SLOT(doOK())); + connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel())); + connect(ui.post, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double))); + connect(ui.pnoise, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double))); + connect(ui.accl, SIGNAL(valueChanged(double)), this, SLOT(settingsChanged(double))); + show(); + } + virtual ~FilterControls() {} + void showEvent ( QShowEvent * event ) { + show(); + } + + void Initialize(QWidget *parent, IFilter* ptr) { + } + + bool settingsDirty; + Ui::KalmanUICFilterControls ui; + +public slots: + void doOK(); + void doCancel(); + void settingsChanged(double unused) { + settingsDirty = true; + } +}; + +#endif diff --git a/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui new file mode 100644 index 00000000..7b71712a --- /dev/null +++ b/ftnoir_filter_kalman/ftnoir_kalman_filtercontrols.ui @@ -0,0 +1,211 @@ + + + KalmanUICFilterControls + + + Qt::ApplicationModal + + + + 0 + 0 + 334 + 100 + + + + + 0 + 0 + + + + Filter settings + + + + + images/facetracknoir.png + images/facetracknoir.png + + + Qt::LeftToRight + + + false + + + + + + + + 173 + 70 + 73 + 25 + + + + OK + + + + + + 250 + 70 + 73 + 25 + + + + Cancel + + + + + + 9 + 30 + 169 + 16 + + + + process-noise-covariance + + + + + + 180 + 26 + 150 + 22 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + 14 + + + 1.000000000000000 + + + 0.000001000000000 + + + 0.500000000000000 + + + + + + 180 + 6 + 150 + 22 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + 14 + + + 1.000000000000000 + + + 0.000001000000000 + + + 0.500000000000000 + + + + + + 9 + 10 + 165 + 16 + + + + post-error-matrix + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + 0 + + + + + + 9 + 55 + 109 + 16 + + + + accel-coefficient + + + + + + 181 + 47 + 150 + 22 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + 6 + + + 1000000000.000000000000000 + + + 0.000001000000000 + + + 1.000000000000000 + + + + + + + + 10 + + + 10 + + + false + + + false + + + true + + + + startEngineClicked() + stopEngineClicked() + cameraSettingsClicked() + + diff --git a/ftnoir_filter_kalman/images/filter-16-ac.png b/ftnoir_filter_kalman/images/filter-16-ac.png new file mode 100644 index 00000000..d263db2d Binary files /dev/null and b/ftnoir_filter_kalman/images/filter-16-ac.png differ diff --git a/ftnoir_filter_kalman/kalman-filter.qrc b/ftnoir_filter_kalman/kalman-filter.qrc new file mode 100644 index 00000000..9a7d75fa --- /dev/null +++ b/ftnoir_filter_kalman/kalman-filter.qrc @@ -0,0 +1,5 @@ + + + images/filter-16-ac.png + + diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp new file mode 100644 index 00000000..92c3fc27 --- /dev/null +++ b/ftnoir_filter_kalman/kalman.cpp @@ -0,0 +1,153 @@ +/* Copyright (c) 2013 Stanisław Halik + * + * 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_filter_kalman.h" +#include "facetracknoir/global-settings.h" +#include +#include + +void kalman_load_settings(FTNoIR_Filter& self) { + 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("ftnoir-filter-kalman"); + self.process_noise_covariance_matrix_all_values = iniFile.value("process-noise-covariance-matrix-all-values", "1e-14").toDouble(); + self.posteriori_error_covariance_matrix_all_values = iniFile.value("posteriori-error-covariance-matrix-all-values", "1e-12").toDouble(); + self.accl = iniFile.value("accel-coefficient", DEFAULT_ACCL).toDouble(); + iniFile.endGroup(); +} + +void kalman_save_settings(FilterControls& self) { + 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("ftnoir-filter-kalman"); + iniFile.setValue("process-noise-covariance-matrix-all-values", self.ui.pnoise->value()); + iniFile.setValue("posteriori-error-covariance-matrix-all-values", self.ui.post->value()); + iniFile.setValue("accel-coefficient", self.ui.accl->value()); + iniFile.endGroup(); +} + +FTNoIR_Filter::FTNoIR_Filter() { + kalman_load_settings(*this); + Initialize(); +} + +// the following was written by Donovan Baarda +// https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b +// minor changes to order of magnitude -sh +void FTNoIR_Filter::Initialize() { + kalman.init(12, 6, 0, CV_64F); + double accel_variance = 1e-2; + kalman.transitionMatrix = *(cv::Mat_(12, 12) << + 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1); + double a = 0.25 * accel_variance; + double b = 0.5 * accel_variance; + double c = 1.0 * accel_variance; + kalman.processNoiseCov = (cv::Mat_(12, 12) << + a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, + 0, a, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, + 0, 0, a, 0, 0, 0, 0, 0, b, 0, 0, 0, + 0, 0, 0, a, 0, 0, 0, 0, 0, b, 0, 0, + 0, 0, 0, 0, a, 0, 0, 0, 0, 0, b, 0, + 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, b, + b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, + 0, b, 0, 0, 0, 0, 0, c, 0, 0, 0, 0, + 0, 0, b, 0, 0, 0, 0, 0, c, 0, 0, 0, + 0, 0, 0, b, 0, 0, 0, 0, 0, c, 0, 0, + 0, 0, 0, 0, b, 0, 0, 0, 0, 0, c, 0, + 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, c); + cv::setIdentity(kalman.measurementMatrix); + cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(accl)); + cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e-4)); +} + +void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget) { + cv::Mat output = kalman.predict(); + if (newTarget) { + cv::Mat measurement(6, 1, CV_64F); + measurement.at(0) = target_camera_position->yaw; + measurement.at(1) = target_camera_position->pitch; + measurement.at(2) = target_camera_position->roll; + measurement.at(3) = target_camera_position->x; + measurement.at(4) = target_camera_position->y; + measurement.at(5) = target_camera_position->z; + kalman.correct(measurement); + } + new_camera_position->yaw = output.at(0); + new_camera_position->pitch = output.at(1); + new_camera_position->roll = output.at(2); + new_camera_position->x = output.at(3); + new_camera_position->y = output.at(4); + new_camera_position->z = output.at(5); + target_camera_position->yaw = output.at(0); + target_camera_position->pitch = output.at(1); + target_camera_position->roll = output.at(2); + target_camera_position->x = output.at(3); + target_camera_position->y = output.at(4); + target_camera_position->z = output.at(5); +} + +void FilterControls::doOK() { + kalman_save_settings(*this); + close(); +} + +void FilterControls::doCancel() { + if (settingsDirty) { + int ret = QMessageBox::question ( this, "Settings have changed", "Do you want to save the settings?", QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel, QMessageBox::Discard ); + + qDebug() << "doCancel says: answer =" << ret; + + switch (ret) { + case QMessageBox::Save: + kalman_save_settings(*this); + this->close(); + break; + case QMessageBox::Discard: + this->close(); + break; + case QMessageBox::Cancel: + // Cancel was clicked + break; + default: + // should never be reached + break; + } + } + else { + this->close(); + } +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata() +{ + return new FTNoIR_FilterDll; +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT void* CALLING_CONVENTION GetConstructor() +{ + return (IFilter*) new FTNoIR_Filter; +} + +extern "C" FTNOIR_FILTER_BASE_EXPORT void* CALLING_CONVENTION GetDialog() { + return (IFilterDialog*) new FilterControls; +} -- cgit v1.2.3