cv/calibrator: limit samples at similar positions

Having yaw and pitch as a tuple, let N be the granularity. We're now only
allowing one sample per the granularity level.

Granularity -- "spacing_in_degrees" has a value of 3 degrees. For now the
values must be integral.

Since we're only allowing (yaw, pitch) tuples of given granularity, the
following get treated as distinct:

(0; 0), (0; 3), (0; 6), (1; 42), (3; 3)

The tuple value order can be swapped. There's nothing significant as for what's
pitch and what's yaw.

We drop the remainder between the yaw/pitch value so (0, 0) is index 0, (0; N)
is index 1, (0; 2N) index 3, etc.

This should prevent the calibration function from biasing itself when the user
keeps still during the procedure.

1 files changed, 15 insertions, 2 deletions

diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp
index cfde0051..76fb5db9 100644
--- a/cv/translation-calibrator.hpp
+++ b/cv/translation-calibrator.hpp
@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
+#include <vector>
 
 //-----------------------------------------------------------------------------
 // Calibrates the translation from head to model = t_MH
@@ -19,7 +20,7 @@
 class TranslationCalibrator
 {
 public:
-    TranslationCalibrator();
+    TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof);
 
     // reset the calibration process
     void reset();
@@ -31,6 +32,18 @@ public:
     cv::Vec3f get_estimate();
 
 private:
+    bool check_bucket(const cv::Matx33d& R_CM_k);
+
     cv::Matx66f P;  // normalized precision matrix = inverse covariance
     cv::Vec6f y;    // P*(-t_MH, t_CH)
+
+    // note, bin count's so small we don't need a bloom filter
+    std::vector<bool> used_poses;
+
+    static constexpr int spacing_in_degrees = 3;
+
+    // this allows allows us up to +-180 for yaw and pitch
+    static constexpr int bin_count = 361*360 / (spacing_in_degrees*spacing_in_degrees) + 1;
+
+    unsigned yaw_rdof, pitch_rdof, nsamples;
 };