summaryrefslogtreecommitdiffhomepage
path: root/compat
diff options
context:
space:
mode:
Diffstat (limited to 'compat')
-rw-r--r--compat/correlation-calibrator.cpp166
-rw-r--r--compat/correlation-calibrator.hpp76
2 files changed, 0 insertions, 242 deletions
diff --git a/compat/correlation-calibrator.cpp b/compat/correlation-calibrator.cpp
deleted file mode 100644
index aef804d2..00000000
--- a/compat/correlation-calibrator.cpp
+++ /dev/null
@@ -1,166 +0,0 @@
-#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
deleted file mode 100644
index b44a2312..00000000
--- a/compat/correlation-calibrator.hpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#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;