2 files changed, 242 insertions, 0 deletions
diff --git a/compat/correlation-calibrator.cpp b/compat/correlation-calibrator.cpp
new file mode 100644
index 00000000..aef804d2
--- /dev/null
+++ b/compat/correlation-calibrator.cpp
@@ -0,0 +1,166 @@
+#include "correlation-calibrator.hpp"
+#include "variance.hpp"
+#include "compat/math.hpp"
+
+#include <cmath>
+#include <iterator>
+
+#include <QDebug>
+
+#define DEBUG_PRINT
+#ifdef DEBUG_PRINT
+#   include <cstdio>
+#   include <cwchar>
+    using std::fwprintf;
+    using std::fflush;
+#endif
+
+using namespace correlation_calibrator_impl;
+
+correlation_calibrator::correlation_calibrator()
+{
+}
+
+static constexpr unsigned nbuckets[6] =
+{
+    x_nbuckets,
+    y_nbuckets,
+    z_nbuckets,
+
+    yaw_nbuckets,
+    pitch_nbuckets,
+    roll_nbuckets,
+};
+
+static constexpr double spacing[6] =
+{
+    translation_spacing,
+    translation_spacing,
+    translation_spacing,
+
+    yaw_spacing_in_degrees,
+    pitch_spacing_in_degrees,
+    roll_spacing_in_degrees,
+};
+
+static constexpr wchar_t const* const names[6] {
+    L"x", L"y", L"z",
+    L"yaw", L"pitch", L"roll",
+};
+
+bool correlation_calibrator::check_buckets(const vec6 &data)
+{
+    bool ret = false;
+    unsigned pos[6];
+
+    for (unsigned k = 0; k < 6; k++)
+    {
+        const double val = clamp(data[k], min[k], max[k]);
+        pos[k] = (val-min[k])/spacing[k];
+
+        if (pos[k] >= nbuckets[k])
+        {
+            qDebug() << "idx" << k
+                     << "bucket" << (int)pos[k]
+                     << "outside bounds" << nbuckets[k];
+
+            return false;
+        }
+
+        if (!buckets[k][pos[k]])
+            ret = true;
+
+        buckets[k][pos[k]] = true;
+    }
+
+    if (ret)
+        for (unsigned k = 0; k < 6; k++)
+            buckets[k][pos[k]] = true;
+
+    return ret;
+}
+
+void correlation_calibrator::input(const vec6& data_)
+{
+    if (!check_buckets(data_))
+        return;
+
+    data.push_back(data_);
+}
+
+mat66 correlation_calibrator::get_coefficients() const
+{
+    if (data.size() < min_samples)
+    {
+        qDebug() << "correlation-calibrator: not enough data";
+
+        mat66 ret;
+        for (unsigned k = 0; k < 6; k++)
+            ret(k, k) = 1;
+        return ret;
+    }
+
+    variance vs[6];
+    vec6 devs, means;
+
+    for (const vec6& x : data)
+        for (unsigned i = 0; i < 6; i++)
+            vs[i].input(x(i));
+
+    for (unsigned i = 0; i < 6; i++)
+    {
+        means(i) = vs[i].avg();
+        devs(i) = vs[i].stddev();
+
+        constexpr double EPS = 1e-4;
+
+        if (devs(i) < EPS)
+            devs(i) = EPS;
+    }
+
+    mat66 cs;
+
+    for (const vec6& x : data)
+        for (unsigned k = 0; k < 6; k++)
+        {
+            for (unsigned idx = 0; idx < 6; idx++)
+            {
+                const double zi = (x(idx) - means(idx)),
+                             zk = (x(k) - means(k));
+
+                cs(idx, k) += zi * zk / (devs(k)*devs(k));
+            }
+        }
+
+    cs = cs * (1./(data.size() - 1));
+
+#if defined DEBUG_PRINT
+    fwprintf(stderr, L"v:change-of h:due-to\n");
+    fwprintf(stderr, L"%10s ", L"");
+    for (unsigned k = 0; k < 6; k++)
+        fwprintf(stderr, L"%10s", names[k]);
+    fwprintf(stderr, L"\n");
+
+    for (unsigned i = 0; i < 6; i++)
+    {
+        fwprintf(stderr, L"%10s ", names[i]);
+        for (unsigned k = 0; k < 6; k++)
+            fwprintf(stderr, L"%10.3f", cs(i, k));
+        fwprintf(stderr, L"\n");
+    }
+    fflush(stderr);
+#endif
+
+    for (unsigned k = 0; k < 6; k++)
+        cs(k, k) = 1;
+
+    // derivations from
+    // https://www.thoughtco.com/how-to-calculate-the-correlation-coefficient-3126228
+
+    return cs;
+}
+
+unsigned correlation_calibrator::sample_count() const
+{
+    return data.size();
+}
diff --git a/compat/correlation-calibrator.hpp b/compat/correlation-calibrator.hpp
new file mode 100644
index 00000000..b44a2312
--- /dev/null
+++ b/compat/correlation-calibrator.hpp
@@ -0,0 +1,76 @@
+#pragma once
+
+#include "simple-mat.hpp"
+#include <array>
+#include <vector>
+#include <tuple>
+
+#include "export.hpp"
+
+namespace correlation_calibrator_impl {
+
+static constexpr inline double min[6] = {
+    -50,
+    -50,
+    250,
+
+    -180,
+    -180,
+    -180,
+};
+
+static constexpr inline double max[6] = {
+    50,
+    50,
+    250,
+
+    180,
+    180,
+    180,
+};
+
+static constexpr inline double yaw_spacing_in_degrees = 1.5;
+static constexpr inline double pitch_spacing_in_degrees = 1;
+static constexpr inline double roll_spacing_in_degrees = 1;
+
+static constexpr inline unsigned yaw_nbuckets   = 1+ 360./yaw_spacing_in_degrees;
+static constexpr inline unsigned pitch_nbuckets = 1+ 360./pitch_spacing_in_degrees;
+static constexpr inline unsigned roll_nbuckets  = 1+ 360./roll_spacing_in_degrees;
+
+static constexpr inline double translation_spacing = .25;
+static constexpr inline unsigned x_nbuckets = 1+ (max[0]-min[0])/translation_spacing;
+static constexpr inline unsigned y_nbuckets = 1+ (max[1]-min[1])/translation_spacing;
+static constexpr inline unsigned z_nbuckets = 1+ (max[2]-min[2])/translation_spacing;
+
+using vec6 = Mat<double, 6, 1>;
+using mat66 = Mat<double, 6, 6>;
+
+class OTR_COMPAT_EXPORT correlation_calibrator final
+{
+    // careful to avoid vector copies
+    std::array<std::vector<bool>, 6> buckets =
+    {
+        std::vector<bool>(x_nbuckets,       false),
+        std::vector<bool>(y_nbuckets,       false),
+        std::vector<bool>(z_nbuckets,       false),
+        std::vector<bool>(yaw_nbuckets,     false),
+        std::vector<bool>(pitch_nbuckets,   false),
+        std::vector<bool>(roll_nbuckets,    false),
+    };
+
+    std::vector<vec6> data;
+
+    bool check_buckets(const vec6& data);
+
+public:
+    correlation_calibrator();
+    void input(const vec6& data);
+    mat66 get_coefficients() const;
+    unsigned sample_count() const;
+
+    static constexpr inline unsigned min_samples = 25;
+};
+
+} // ns correlation_calibrator_impl
+
+using correlation_calibrator_impl::correlation_calibrator;