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

1 files changed, 23 insertions, 18 deletions

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;
 };