summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--compat/correlation-calibrator.cpp166
-rw-r--r--compat/correlation-calibrator.hpp76
-rw-r--r--tracker-pt/ftnoir_tracker_pt_dialog.cpp15
-rw-r--r--tracker-pt/ftnoir_tracker_pt_dialog.h2
-rw-r--r--tracker-pt/ftnoir_tracker_pt_settings.h21
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", ""),