diff options
| -rw-r--r-- | compat/correlation-calibrator.cpp | 166 | ||||
| -rw-r--r-- | compat/correlation-calibrator.hpp | 76 | ||||
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt_dialog.cpp | 15 | ||||
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt_dialog.h | 2 | ||||
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt_settings.h | 21 |
5 files changed, 280 insertions, 0 deletions
diff --git a/compat/correlation-calibrator.cpp b/compat/correlation-calibrator.cpp new file mode 100644 index 00000000..1d3339d3 --- /dev/null +++ b/compat/correlation-calibrator.cpp @@ -0,0 +1,166 @@ +#include "correlation-calibrator.hpp" +#include "variance.hpp" +#include "util.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; diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.cpp b/tracker-pt/ftnoir_tracker_pt_dialog.cpp index 2c3cbd07..35a029df 100644 --- a/tracker-pt/ftnoir_tracker_pt_dialog.cpp +++ b/tracker-pt/ftnoir_tracker_pt_dialog.cpp @@ -131,6 +131,8 @@ void TrackerDialog_PT::startstop_trans_calib(bool start) if (start) { + c_calib = {}; + qDebug() << "pt: starting translation calibration"; calib_timer.start(); trans_calib.reset(); @@ -142,6 +144,9 @@ void TrackerDialog_PT::startstop_trans_calib(bool start) } else { + // XXX reenable after build + // (void) c_calib.get_coefficients(); + calib_timer.stop(); qDebug() << "pt: stopping translation calibration"; { @@ -237,8 +242,18 @@ void TrackerDialog_PT::trans_calib_step() if (tracker) { + // XXX reenable after build + if (false) + { + + Mat<double, 6, 1> m; + tracker->data(m); + c_calib.input(m); + } + Affine X_CM = tracker->pose(); trans_calib.update(X_CM.R, X_CM.t); + } else startstop_trans_calib(false); diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.h b/tracker-pt/ftnoir_tracker_pt_dialog.h index c1c25fb0..bae03adc 100644 --- a/tracker-pt/ftnoir_tracker_pt_dialog.h +++ b/tracker-pt/ftnoir_tracker_pt_dialog.h @@ -14,6 +14,7 @@ #include "ui_FTNoIR_PT_Controls.h" #include "cv/translation-calibrator.hpp" #include "cv/video-widget.hpp" +#include "compat/correlation-calibrator.hpp" #include <QTimer> #include <QMutex> @@ -44,6 +45,7 @@ private: Tracker_PT* tracker; QTimer timer, calib_timer; TranslationCalibrator trans_calib; + correlation_calibrator c_calib; QMutex calibrator_mutex; Ui::UICPTClientControls ui; diff --git a/tracker-pt/ftnoir_tracker_pt_settings.h b/tracker-pt/ftnoir_tracker_pt_settings.h index 71b98fc2..7fa560d2 100644 --- a/tracker-pt/ftnoir_tracker_pt_settings.h +++ b/tracker-pt/ftnoir_tracker_pt_settings.h @@ -46,6 +46,27 @@ struct settings_pt : opts value<slider_value> threshold_slider; +#define XSTR(x) #x + +#define M(name, val) \ + { b, "interconnect-m" XSTR(name), (val) } +#define A(name) M(name, 0), +#define B(name) M(name, 1), + + value<double> interconnect[6][6] { + { B(00) A(01) A(02) A(03) A(04) A(05) }, + { A(10) B(11) A(12) A(13) A(14) A(15) }, + { A(20) A(21) B(22) A(23) A(24) A(25) }, + { A(30) A(31) A(32) B(33) A(34) A(35) }, + { A(40) A(41) A(42) A(43) B(44) A(45) }, + { A(50) A(51) A(52) A(53) A(54) B(55) }, + }; + +#undef M +#undef A +#undef B +#undef XSTR + settings_pt() : opts("tracker-pt"), camera_name(b, "camera-name", ""), |