tracker/nn: Tweaks, refactoring, a deadzone filtering and support for uncertainty estimation

* Add rudimentary test for two functions .. maybe more in future
* Fix the rotation correction from vertical translation
* Move preview class to new files
* Move neural network model adapters to new files
* Add utility functions for opencv
* Query the model inputs/outputs by name to see what is available
* Supports outputs for standard deviation of the data distribution -
  What you get if you let your model output the full parameters of a
  gaussian distribution (depending on the inputs) and fit it with
  negative log likelihood loss.
* Disabled support for sequence models
* Add support for detection of eye open/close classification.
  Scale uncertainty estimate up if eyes closed
* Add a deadzone filter which activates if the model supports uncertainty
  quantification. The deadzone scales becomes larger the more uncertain
  the model/data are. This is mostly supposed to be useful to suppress
  large estimate errors when the user blinks with the eyes
* Fix distance being twice of what it should have been

1 files changed, 132 insertions, 0 deletions

diff --git a/tracker-neuralnet/unscented_trafo.h b/tracker-neuralnet/unscented_trafo.h
new file mode 100644
index 00000000..267aa969
--- /dev/null
+++ b/tracker-neuralnet/unscented_trafo.h
@@ -0,0 +1,132 @@
+#pragma once
+
+#include <algorithm>
+#include <opencv2/core.hpp>
+#include <opencv2/core/base.hpp>
+#include <opencv2/core/quaternion.hpp>
+
+#include <cmath>
+#include <vector>
+
+#include "opencv_contrib.h"
+
+namespace ukf_cv
+{
+
+using namespace cvcontrib;
+
+template<int dim, int otherdim = dim>
+using SigmaPoints = std::array<cv::Vec<float,otherdim>,dim*2+1>;
+
+
+// Ported from 
+// https://filterpy.readthedocs.io/en/latest/_modules/filterpy/kalman/sigma_points.html
+// Excerpt from the original docu:
+// "
+
+// Generates sigma points and weights according to Van der Merwe's
+// 2004 dissertation[1] for the UnscentedKalmanFilter class.. It
+// parametizes the sigma points using alpha, beta, kappa terms, and
+// is the version seen in most publications.
+
+// Unless you know better, this should be your default choice.
+
+// alpha : float
+//     Determins the spread of the sigma points around the mean.
+//     Usually a small positive value (1e-3) according to [3].
+
+// beta : float
+//     Incorporates prior knowledge of the distribution of the mean. For
+//     Gaussian x beta=2 is optimal, according to [3].
+
+// kappa : float, default=0.0
+//     Secondary scaling parameter usually set to 0 according to [4],
+//     or to 3-n according to [5].
+
+// Reference
+// .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
+//        Inference in Dynamic State-Space Models" (Doctoral dissertation)
+
+// "
+template<int dim>
+class MerweScaledSigmaPoints
+{
+public:
+    static constexpr int num_sigmas = 2*dim+1;
+    
+    using Vector = cv::Vec<float,dim>;
+    using Matrix = cv::Matx<float,dim,dim>;
+
+    MerweScaledSigmaPoints(float alpha = 0.01, float beta = 2., int kappa = 3-dim)
+    {
+        lambda = alpha*alpha * (dim + kappa) - dim;
+        const float c = .5 / (dim + lambda);
+        Wc_i = c;
+        Wm_i = c;
+        Wm_0 = lambda / (dim+lambda);
+        Wc_0 = Wm_0 + (1.-alpha*alpha + beta);
+    }
+
+    SigmaPoints<dim> compute_sigmas(const Vector &mu, const Matrix &mat, bool is_tril_factor) const
+    {
+        const Matrix triu_factor = is_tril_factor ? mat.t() : cholesky(mat).t();
+
+        const Matrix U = triu_factor*std::sqrt(lambda+dim);
+
+        SigmaPoints<dim> sigmas;
+
+        sigmas[0] = mu;
+        for (int k=0; k<dim; ++k)
+        {
+            sigmas[k+1] = to_vec(mu + U.row(k).t());
+            sigmas[dim+k+1] = to_vec(mu - U.row(k).t());
+        }
+        return sigmas;
+    }
+
+    template<int otherdim>
+    std::tuple<cv::Vec<float,otherdim> , cv::Matx<float,otherdim,otherdim>> compute_statistics(const SigmaPoints<dim,otherdim> &sigmas) const
+    {
+        cv::Vec<float,otherdim> mu{}; // Zero initializes
+        for (size_t i=0; i<sigmas.size(); ++i)
+        {
+            mu += to_vec((i==0 ? Wm_0 : Wm_i) * sigmas[i]);
+        }
+
+        cv::Matx<float,otherdim,otherdim> cov{};
+        for (size_t i=0; i<sigmas.size(); ++i)
+        {
+            const auto p = sigmas[i] - mu;
+            cov += (i==0 ? Wc_0 : Wc_i)*p*p.t();
+        }
+
+        return { mu, cov };
+    }
+
+    template<int otherdim>
+    cv::Matx<float,dim,otherdim> compute_cov(const SigmaPoints<dim,dim> &sigmas, const SigmaPoints<dim,otherdim> &othersigmas) const
+    {
+        cv::Vec<float,dim> mu{}; // Zero initializes
+        cv::Vec<float,otherdim> mu_other{}; // Zero initializes
+        for (size_t i=0; i<sigmas.size(); ++i)
+        {
+            mu += to_vec((i==0 ? Wm_0 : Wm_i) * sigmas[i]);
+            mu_other += to_vec((i==0 ? Wm_0 : Wm_i) * othersigmas[i]);
+        }
+
+        cv::Matx<float,dim,otherdim> cov{};
+        for (size_t i=0; i<sigmas.size(); ++i)
+        {
+            const auto p = sigmas[i] - mu;
+            const auto q = othersigmas[i] - mu_other;
+            cov += (i==0 ? Wc_0 : Wc_i)*p*q.t();
+        }
+
+        return cov;
+    }
+private:
+    float Wc_i, Wm_i, Wm_0, Wc_0, lambda;
+};
+
+
+} // namespace ukf_cv
\ No newline at end of file