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.

2 files changed, 66 insertions, 3 deletions

diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
index a5df5b5f..75526815 100644
--- a/cv/translation-calibrator.cpp
+++ b/cv/translation-calibrator.cpp
@@ -6,8 +6,13 @@
  */
 
 #include "translation-calibrator.hpp"
+#include "compat/euler.hpp"
+#include "compat/util.hpp"
 
-TranslationCalibrator::TranslationCalibrator()
+#include <cmath>
+
+TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof) :
+    yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof)
 {
     reset();
 }
@@ -16,10 +21,18 @@ void TranslationCalibrator::reset()
 {
     P = cv::Matx66f::zeros();
     y = cv::Vec6f(0,0,0, 0,0,0);
+
+    used_poses = std::vector<bool>(bin_count, false);
+    nsamples = 0;
 }
 
 void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k)
 {
+    if (!check_bucket(R_CM_k))
+        return;
+
+    nsamples++;
+
     cv::Matx<double, 6,3> H_k_T = cv::Matx<double, 6,3>::zeros();
     for (int i=0; i<3; ++i) {
         for (int j=0; j<3; ++j) {
@@ -37,5 +50,42 @@ void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t
 cv::Vec3f TranslationCalibrator::get_estimate()
 {
     cv::Vec6f x = P.inv() * y;
+
+    qDebug() << "calibrator:" << nsamples << "samples total";
+
     return cv::Vec3f(-x[0], -x[1], -x[2]);
 }
+
+bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
+{
+    const int idx = progn(
+        using namespace euler;
+        static constexpr double r2d = 180/M_PI;
+
+        rmat r;
+        for (unsigned j = 0; j < 3; j++)
+            for (unsigned i = 0; i < 3; i++)
+                r(j, i) = R_CM_k(j, i);
+
+        const euler_t ypr = rmat_to_euler(r) * r2d;
+
+        const int yaw = iround(ypr(yaw_rdof) + 180)/spacing_in_degrees;
+        const int pitch = iround(ypr(pitch_rdof) + 180)/spacing_in_degrees;
+        return pitch * 360/spacing_in_degrees + yaw;
+    );
+
+    if (idx >= 0 && idx < bin_count)
+    {
+        if (used_poses[idx])
+        {
+            return false;
+        }
+        else
+        {
+            used_poses[idx] = true;
+            return true;
+        }
+    }
+
+    return false;
+}
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;
 };