summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui88
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela.cpp16
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela.h7
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp4
-rw-r--r--ftnoir_filter_kalman/ftnoir_filter_kalman.h33
-rw-r--r--ftnoir_filter_kalman/kalman.cpp60
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 @@
<x>0</x>
<y>0</y>
<width>347</width>
- <height>268</height>
+ <height>339</height>
</rect>
</property>
<property name="sizePolicy">
@@ -109,7 +109,7 @@
<enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
</property>
<property name="decimals">
- <number>3</number>
+ <number>4</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
@@ -147,7 +147,7 @@
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="decimals">
- <number>3</number>
+ <number>4</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
@@ -160,7 +160,7 @@
</property>
</widget>
</item>
- <item row="2" column="0">
+ <item row="4" column="0">
<widget class="QLabel" name="lblSensYaw_5">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Expanding">
@@ -189,7 +189,7 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="2" column="1">
+ <item row="4" column="1">
<widget class="QSpinBox" name="spinZoom">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Maximum">
@@ -229,7 +229,7 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="3" column="0">
+ <item row="5" column="0">
<widget class="QLabel" name="lblSensYaw_6">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Expanding">
@@ -258,7 +258,7 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="3" column="1">
+ <item row="5" column="1">
<widget class="QDoubleSpinBox" name="deadzone">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Maximum">
@@ -286,14 +286,14 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="4" column="0">
+ <item row="6" column="0">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Exponent</string>
</property>
</widget>
</item>
- <item row="4" column="1">
+ <item row="6" column="1">
<widget class="QDoubleSpinBox" name="expt">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
@@ -315,6 +315,76 @@ background:none;</string>
</property>
</widget>
</item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>Order #2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="order_2nd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="correctionMode">
+ <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
+ </property>
+ <property name="decimals">
+ <number>4</number>
+ </property>
+ <property name="minimum">
+ <double>0.100000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65535.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="order_3rd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="correctionMode">
+ <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
+ </property>
+ <property name="decimals">
+ <number>4</number>
+ </property>
+ <property name="minimum">
+ <double>0.100000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65535.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>Order #3</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
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 <QString>
#include <QIcon>
#include <QWidget>
+#include <QElapsedTimer>
#include <QObject>
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 <QDebug>
#include <math.h>
-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 <abo@minkirri.apana.org.au>
// 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_<double>(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<typename T>
+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<double>(i) = target_camera_position[i+3];
measurement.at<double>(i+3) = target_camera_position[i];
}
kalman.correct(measurement);
- }
-
- for (int i = 0; i < 3; i++) {
- new_camera_position[i] = output.at<double>(i+3);
- new_camera_position[i+3] = output.at<double>(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<double>((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;
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-17 01:58:08 +0000