summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--cmake/FindEigen3.cmake8
-rw-r--r--filter-kalman/kalman.cpp33
-rw-r--r--filter-kalman/kalman.h83
3 files changed, 63 insertions, 61 deletions
diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake
index 2206ce43..b43208f9 100644
--- a/cmake/FindEigen3.cmake
+++ b/cmake/FindEigen3.cmake
@@ -80,5 +80,11 @@ else (EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
if(EIGEN3_FOUND)
- add_definitions(-DEIGEN_MPL2_ONLY)
+ add_definitions(
+ -DEIGEN_MPL2_ONLY=1
+ -DEIGEN_MAX_CPP_VER=17
+ -DEIGEN_DEFAULT_DENSE_INDEX_TYPE=int
+ -DEIGEN_NO_DEBUG=1
+ -DEIGEN_DONT_PARALLELIZE=1
+ )
endif()
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 83898853..472f01f0 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -76,6 +76,10 @@ void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt)
//qDebug() << "alpha = " << alpha;
}
+void DeadzoneFilter::reset()
+{
+ last_output = PoseVector::Zero();
+}
PoseVector DeadzoneFilter::filter(const PoseVector &input)
{
@@ -151,7 +155,8 @@ PoseVector kalman::do_kalman_filter(const PoseVector &input, double dt, bool new
-kalman::kalman() {
+kalman::kalman()
+{
reset();
}
@@ -289,5 +294,31 @@ void dialog_kalman::doCancel()
close();
}
+double settings::map_slider_value(const slider_value& v_)
+{
+ const double v = v_;
+#if 0
+ //return std::pow(10., v * 4. - 3.);
+#else
+ constexpr int min_log10 = -3;
+ constexpr int max_log10 = 1;
+ constexpr int num_divisions = max_log10 - min_log10;
+ /* ascii art representation of slider
+ // ----- // ------// ------// ------- // 4 divisions
+ -3 - 2 -1 0 1 power of 10
+ | |
+ | f + left_side_log10
+ |
+ left_side_log10
+ */
+ const int k = v * num_divisions; // in which division are we?!
+ const double f = v * num_divisions - k; // where in the division are we?!
+ const double ff = f * 9. + 1.;
+ const double multiplier = int(ff * 10.) / 10.;
+ const int left_side_log10 = min_log10 + k;
+ const double val = std::pow(10., left_side_log10) * multiplier;
+ return val;
+#endif
+}
OPENTRACK_DECLARE_FILTER(kalman, dialog_kalman, kalmanDll)
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 094e106c..30f9e90f 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -6,20 +6,26 @@
* copyright notice and this permission notice appear in all copies.
*/
-#include "ui_ftnoir_kalman_filtercontrols.h"
+#include "compat/timer.hpp"
#include "api/plugin-api.hpp"
#include "options/options.hpp"
using namespace options;
-#include "compat/timer.hpp"
+
+// Eigen can't check for SSE3 on MSVC
+#if defined _MSC_VER && defined __SSE2__
+# define EIGEN_VECTORIZE_SSE3
+// this hardware is 10 years old
+# define EIGEN_VECTORIZE_SSE4_1
+# define EIGEN_VECTORIZE_SSE4_2
+#endif
#include <Eigen/Core>
#include <Eigen/LU>
+#include "ui_ftnoir_kalman_filtercontrols.h"
#include <QString>
#include <QWidget>
-#include <atomic>
-
static constexpr inline int NUM_STATE_DOF = 12;
static constexpr inline int NUM_MEASUREMENT_DOF = 6;
// These vectors are compile time fixed size, stack allocated
@@ -48,11 +54,10 @@ struct KalmanFilter
state_prior;
PoseVector
innovation;
+
void init();
void time_update();
void measurement_update(const PoseVector &measurement);
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct KalmanProcessNoiseScaler
@@ -63,32 +68,23 @@ struct KalmanProcessNoiseScaler
base_cov; // baseline (unscaled) process noise covariance matrix
void init();
void update(KalmanFilter &kf, double dt);
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct DeadzoneFilter
{
- PoseVector
- last_output,
- dz_size;
- DeadzoneFilter() :
- last_output(PoseVector::Zero()),
- dz_size(PoseVector::Zero())
- {}
- void reset() {
- last_output = PoseVector::Zero();
- }
- PoseVector filter(const PoseVector &input);
+ PoseVector last_output { PoseVector::Zero() },
+ dz_size { PoseVector::Zero() };
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ DeadzoneFilter() = default;
+ void reset();
+ PoseVector filter(const PoseVector &input);
};
struct settings : opts {
- value<slider_value> noise_rot_slider_value;
- value<slider_value> noise_pos_slider_value;
+ value<slider_value> noise_rot_slider_value { b, "noise-rotation-slider", { .5, 0, 1 } };
+ value<slider_value> noise_pos_slider_value { b, "noise-position-slider", { .5, 0, 1 } };
static constexpr inline double adaptivity_window_length = 0.25; // seconds
static constexpr inline double deadzone_scale = 8;
@@ -96,39 +92,9 @@ struct settings : opts {
static constexpr inline double process_sigma_pos = 0.5;
static constexpr inline double process_sigma_rot = 0.5;
- static double map_slider_value(const slider_value &v_)
- {
- const double v = v_;
-#if 0
- //return std::pow(10., v * 4. - 3.);
-#else
- constexpr int min_log10 = -3;
- constexpr int max_log10 = 1;
- constexpr int num_divisions = max_log10 - min_log10;
- /* ascii art representation of slider
- // ----- // ------// ------// ------- // 4 divisions
- -3 - 2 -1 0 1 power of 10
- | |
- | f + left_side_log10
- |
- left_side_log10
- */
- const int k = v * num_divisions; // in which division are we?!
- const double f = v * num_divisions - k; // where in the division are we?!
- const double ff = f * 9. + 1.;
- const double multiplier = int(ff * 10.) / 10.;
- const int left_side_log10 = min_log10 + k;
- const double val = std::pow(10., left_side_log10) * multiplier;
- return val;
-#endif
- }
-
- settings() :
- opts("kalman-filter"),
- noise_rot_slider_value(b, "noise-rotation-slider", { .5, 0., 1. }),
- noise_pos_slider_value(b, "noise-position-slider", { .5, 0., 1. })
- {}
+ static double map_slider_value(const slider_value &v);
+ settings() : opts("kalman-filter") {}
};
class kalman : public IFilter
@@ -141,7 +107,8 @@ public:
void reset();
void filter(const double *input, double *output) override;
void center() override { reset(); }
- module_status initialize() { return status_ok(); }
+ module_status initialize() override { return status_ok(); }
+
PoseVector last_input;
Timer timer;
bool first_run;
@@ -151,16 +118,14 @@ public:
KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
DeadzoneFilter dz_filter;
slider_value prev_slider_pos[2];
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class kalmanDll : public Metadata
{
Q_OBJECT
- QString name() { return tr("Kalman"); }
- QIcon icon() { return QIcon(":/images/filter-16.png"); }
+ QString name() override { return tr("Kalman"); }
+ QIcon icon() override { return QIcon(":/images/filter-16.png"); }
};
class dialog_kalman: public IFilterDialog