summaryrefslogtreecommitdiffhomepage
path: root/compat/correlation-calibrator.hpp
diff options
context:
space:
mode:
Diffstat (limited to 'compat/correlation-calibrator.hpp')
-rw-r--r--compat/correlation-calibrator.hpp76
1 files changed, 76 insertions, 0 deletions
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;