cv/tcal: don't count separate roll as sample

2 files changed, 60 insertions, 49 deletions

diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
index d08e0c43..708cc593 100644
--- a/cv/translation-calibrator.cpp
+++ b/cv/translation-calibrator.cpp
@@ -13,8 +13,8 @@
 
 #include <QDebug>
 
-TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof, unsigned roll_rdof) :
-    yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof), roll_rdof(roll_rdof)
+TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof) :
+    yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof)
 {
     reset();
 }
@@ -22,11 +22,10 @@ TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_r
 void TranslationCalibrator::reset()
 {
     P = cv::Matx66f::zeros();
-    y = cv::Vec6f(0,0,0, 0,0,0);
+    y = { 0,0,0, 0,0,0 };
 
-    used_yaw_poses = vec(1 + iround(360 / yaw_spacing_in_degrees), 0);
-    used_pitch_poses = vec(1 + iround(360 / pitch_spacing_in_degrees), 0);
-    used_roll_poses = vec(1 + iround(360 / roll_spacing_in_degrees), 0);
+    used_yaw_poses = vec_i(1 + iround(360 / yaw_spacing_in_degrees), 0);
+    used_pitch_poses = vec_i(1 + iround(360 / pitch_spacing_in_degrees), 0);
 
     nsamples = 0;
 }
@@ -52,34 +51,40 @@ void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t
     y += H_k_T * t_CM_k;
 }
 
-std::tuple<cv::Vec3f, cv::Vec3i> TranslationCalibrator::get_estimate()
+using cv_out_vec = TranslationCalibrator::cv_nsample_vec;
+using cv_in_vec = TranslationCalibrator::cv_cal_vec;
+using tt = TranslationCalibrator::tt;
+
+tt TranslationCalibrator::get_estimate()
 {
     cv::Vec6f x = P.inv() * y;
 
-    unsigned values[3] {};
-    vec* in[] { &used_yaw_poses, &used_pitch_poses, &used_roll_poses };
+    vec_i const* in[num_nsample_axis] = { &used_yaw_poses, &used_pitch_poses, };
+    unsigned nsamples[num_cal_axis] {};
 
-    for (unsigned k = 0; k < 3; k++)
+    for (unsigned k = 0; k < num_nsample_axis; k++)
     {
-        const vec& data = *in[k];
+        vec_i const& data = *in[k];
         for (unsigned i : data)
             if (i)
-                values[k]++;
+                nsamples[k]++;
     }
 
     qDebug() << "samples total" << nsamples
-             << "yaw" << values[0]
-             << "pitch" << values[1]
-             << "roll" << values[2];
+             << "yaw" << nsamples[0]
+             << "pitch" << nsamples[1];
 
-    return std::make_tuple(cv::Vec3f(-x[0], -x[1], -x[2]),
-                           cv::Vec3i(values[0], values[1], values[2]));
+    return {
+        { -x[0], -x[1], -x[2] },
+        { nsamples[0], nsamples[1] },
+    };
 }
 
+static constexpr inline double r2d = 180/M_PI;
+
 bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
 {
     using namespace euler;
-    constexpr double r2d = 180/M_PI;
 
     rmat r;
     for (unsigned j = 0; j < 3; j++)
@@ -88,27 +93,28 @@ bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
 
     const euler_t ypr = rmat_to_euler(r) * r2d;
 
-    const unsigned yaw_k = iround((ypr(yaw_rdof) + 180)/yaw_spacing_in_degrees);
-    const unsigned pitch_k = iround((ypr(pitch_rdof) + 180)/pitch_spacing_in_degrees);
-    const unsigned roll_k = iround((ypr(roll_rdof) + 180)/roll_spacing_in_degrees);
+    auto array_index = [](double val, double spacing) {
+        return iround((val + 180)/spacing);
+    };
+
+    const unsigned yaw_k = array_index(ypr(yaw_rdof), yaw_spacing_in_degrees);
+    const unsigned pitch_k = array_index(ypr(pitch_rdof), pitch_spacing_in_degrees);
 
     if (yaw_k < used_yaw_poses.size() &&
-        pitch_k < used_pitch_poses.size() &&
-        roll_k < used_roll_poses.size())
+        pitch_k < used_pitch_poses.size())
     {
         used_yaw_poses[yaw_k]++;
         used_pitch_poses[pitch_k]++;
-        used_roll_poses[roll_k]++;
 
         return used_yaw_poses[yaw_k] == 1 ||
-               used_pitch_poses[pitch_k] == 1 ||
-               used_roll_poses[roll_k] == 1;
+               used_pitch_poses[pitch_k] == 1;
     }
     else
-        qDebug() << "calibrator: index out of range"
-                 << "yaw" << yaw_k
-                 << "pitch" << pitch_k
-                 << "roll" << roll_k;
+    {
+        eval_once(qDebug() << "calibrator: index out of range"
+                           << "yaw" << yaw_k
+                           << "pitch" << pitch_k);
 
-    return false;
+        return false;
+    }
 }
diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp
index 774bc7cb..da052526 100644
--- a/cv/translation-calibrator.hpp
+++ b/cv/translation-calibrator.hpp
@@ -19,33 +19,38 @@
 
 class TranslationCalibrator
 {
-public:
-    TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof, unsigned roll_rdof);
+    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)
+
+    using vec_i = std::vector<unsigned>;
+
+    vec_i used_yaw_poses {};
+    vec_i used_pitch_poses {};
+
+    unsigned yaw_rdof, pitch_rdof, nsamples = 0;
 
-    // reset the calibration process
+public:
+    TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof);
     void reset();
 
     // update the current estimate
     void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k);
 
-    // get the current estimate for t_MH
-    std::tuple<cv::Vec3f, cv::Vec3i> get_estimate();
-
-private:
-    bool check_bucket(const cv::Matx33d& R_CM_k);
+    // we're bringing in 3DOF samples but the calibrator only
+    // checks yaw and pitch
 
-    cv::Matx66f P;  // normalized precision matrix = inverse covariance
-    cv::Vec6f y;    // P*(-t_MH, t_CH)
+    static constexpr inline unsigned num_cal_axis = 3;
+    static constexpr inline unsigned num_nsample_axis = 2;
 
-    using vec = std::vector<unsigned>;
+    using cv_cal_vec = cv::Vec<float, num_cal_axis>;
+    using cv_nsample_vec = cv::Vec<unsigned, num_nsample_axis>;
+    using tt = std::tuple<cv_cal_vec, cv_nsample_vec>;
 
-    vec used_yaw_poses;
-    vec used_pitch_poses;
-    vec used_roll_poses;
+    // get the current estimate for t_MH
+    tt get_estimate();
 
-    static constexpr inline double yaw_spacing_in_degrees = 2.5;
+    static constexpr inline double yaw_spacing_in_degrees = 2;
     static constexpr inline double pitch_spacing_in_degrees = 1.5;
-    static constexpr inline double roll_spacing_in_degrees = 3.5;
-
-    unsigned yaw_rdof, pitch_rdof, roll_rdof, nsamples = 0;
 };