From 08f1fcad1c74e25f97641a0ccbd229b267ec528c Mon Sep 17 00:00:00 2001
From: Michael Welter <michael@welter-4d.de>
Date: Sun, 11 Sep 2022 20:55:26 +0200
Subject: 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
---
 tracker-neuralnet/opencv_contrib.h | 120 +++++++++++++++++++++++++++++++++++++
 1 file changed, 120 insertions(+)
 create mode 100644 tracker-neuralnet/opencv_contrib.h

(limited to 'tracker-neuralnet/opencv_contrib.h')

diff --git a/tracker-neuralnet/opencv_contrib.h b/tracker-neuralnet/opencv_contrib.h
new file mode 100644
index 00000000..af92c12f
--- /dev/null
+++ b/tracker-neuralnet/opencv_contrib.h
@@ -0,0 +1,120 @@
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/base.hpp>
+#include <opencv2/core/quaternion.hpp>
+
+// Well eventually it might be a contribution
+
+namespace cvcontrib
+{
+
+
+template<class T>
+cv::Point_<T> as_point(const cv::Size_<T>& s)
+{
+    return { s.width, s.height };
+}
+
+
+template<class T>
+cv::Size_<T> as_size(const cv::Point_<T>& p)
+{
+    return { p.x, p.y };
+}
+
+
+template<int n, int m>
+inline bool allfinite(const cv::Matx<float, n, m> &mat)
+{
+    const size_t sz = mat.rows*mat.cols;
+    for (size_t i=0; i<sz; ++i)
+        if (!std::isfinite(mat.val[i]))
+            return false;
+    return true;
+}
+
+
+// Because compiler refuses to convert it automatically
+template<int n>
+inline cv::Vec<float, n> to_vec(const cv::Matx<float, n, 1>& m)
+{
+    return cv::Vec<float,n>{m.val};
+}
+
+
+template<int n, int m, int o>
+inline void set_minor(cv::Vec<float, m> &dst, const int startrow, const cv::Matx<float, o, 1> &src)
+{
+    assert (startrow>=0 && startrow+n <= dst.rows);
+    for (int row=startrow, i=0; row<startrow+n; ++row,++i)
+    {
+        dst[row] = src(i,0);
+    }
+}
+
+
+template<int nrows, int ncols, int m, int n>
+inline void set_minor(cv::Matx<float, m, n>& dst, const int startrow, int startcol, const cv::Matx<float, nrows, ncols> &src)
+{
+    assert (startrow>=0 && startrow+nrows <= dst.rows);
+    assert (startcol>=0 && startcol+ncols <= dst.cols);
+    for (int row=startrow, i=0; row<startrow+nrows; ++row,++i)
+    {
+        for (int col=startcol, j=0; col<startcol+ncols; ++col,++j)
+        {
+            dst(row, col) = src(i,j);
+        }
+    }
+}
+
+
+inline cv::Quatf identity_quat()
+{
+    return cv::Quatf(1,0,0,0);
+}
+
+
+inline cv::Vec3f toRotVec(const cv::Quatf& q)
+{
+    // This is an improved implementation
+#if 1
+    // w = cos(alpha/2)
+    // xyz = sin(alpha/2)*axis
+    static constexpr float eps = 1.e-12;
+    const cv::Vec3f xyz{q.x, q.y, q.z};
+    const float len = cv::norm(xyz);
+    const float angle = std::atan2(len, q.w)*2.f;
+    return xyz*(angle/(len+eps));
+#else
+    // The opencv implementation fails even the simplest test:
+    // out = toRVec(cv::Quatf{1., 0., 0., 0. });
+    // ASSERT_TRUE(std::isfinite(out[0]) && std::isfinite(out[1]) && std::isfinite(out[2]));    
+    return q.toRotVec();
+#endif
+}
+
+
+inline cv::Vec3f rotate(const cv::Quatf& q, const cv::Vec3f &v)
+{
+    const auto r = q * cv::Quatf{0., v[0], v[1], v[2]} * q.conjugate();
+    return { r.x, r.y, r.z };
+}
+
+
+template<int n>
+inline cv::Matx<float, n, n> cholesky(const cv::Matx<float, n, n>& mat)
+{
+    cv::Matx<float, n, n> l = mat;
+    // Der Code ist die Doku! 
+    // https://github.com/opencv/opencv/blob/4.5.4/modules/core/src/matrix_decomp.cpp#L95
+    cv::Cholesky(l.val, l.cols * sizeof(float), n, nullptr, 0, 0);
+    // It doesn't clear the upper triangle so we do it for it.
+    for (int row=0; row<n; ++row)
+        for (int col=row+1; col<n; ++col)
+            l(row, col) = 0.f;
+    return l;
+}
+
+
+} // namespace cvcontrib
\ No newline at end of file
-- 
cgit v1.2.3