diff options
Diffstat (limited to 'compat')
| -rw-r--r-- | compat/correlation-calibrator.cpp | 166 | ||||
| -rw-r--r-- | compat/correlation-calibrator.hpp | 76 |
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; |