cv/calibrator: return nsamples, separate pitch/yaw spacing

2 files changed, 15 insertions, 9 deletions

diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
index f76fc32e..b1c47a8a 100644
--- a/cv/translation-calibrator.cpp
+++ b/cv/translation-calibrator.cpp
@@ -9,7 +9,10 @@
 #include "compat/euler.hpp"
 #include "compat/util.hpp"
 
-#include <cmath>
+#include <tuple>
+
+constexpr double TranslationCalibrator::pitch_spacing_in_degrees;
+constexpr double TranslationCalibrator::yaw_spacing_in_degrees;
 
 TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof) :
     yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof)
@@ -47,13 +50,13 @@ void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t
     y += H_k_T * t_CM_k;
 }
 
-cv::Vec3f TranslationCalibrator::get_estimate()
+std::tuple<cv::Vec3f, unsigned> TranslationCalibrator::get_estimate()
 {
     cv::Vec6f x = P.inv() * y;
 
     qDebug() << "calibrator:" << nsamples << "samples total";
 
-    return cv::Vec3f(-x[0], -x[1], -x[2]);
+    return std::make_tuple(cv::Vec3f(-x[0], -x[1], -x[2]), nsamples);
 }
 
 bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
@@ -68,9 +71,9 @@ bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
 
     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;
-    const int idx = pitch * 360/spacing_in_degrees + yaw;
+    const int yaw = iround(ypr(yaw_rdof) + 180/yaw_spacing_in_degrees);
+    const int pitch = iround(ypr(pitch_rdof) + 180/pitch_spacing_in_degrees);
+    const int idx = pitch * 360/pitch_spacing_in_degrees + yaw;
 
     if (idx >= 0 && idx < bin_count)
     {
@@ -84,6 +87,8 @@ bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
             return true;
         }
     }
+    else
+        qDebug() << "calibrator: index out of range" << "yaw" << yaw << "pitch" << pitch << "max" << bin_count;
 
     return false;
 }
diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp
index 76fb5db9..2bf73839 100644
--- a/cv/translation-calibrator.hpp
+++ b/cv/translation-calibrator.hpp
@@ -29,7 +29,7 @@ public:
     void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k);
 
     // get the current estimate for t_MH
-    cv::Vec3f get_estimate();
+    std::tuple<cv::Vec3f, unsigned> get_estimate();
 
 private:
     bool check_bucket(const cv::Matx33d& R_CM_k);
@@ -40,10 +40,11 @@ private:
     // 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;
+    static constexpr double yaw_spacing_in_degrees = 2.5;
+    static constexpr double pitch_spacing_in_degrees = 1.5;
 
     // 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;
+    static constexpr int bin_count = 361*360 / (yaw_spacing_in_degrees*pitch_spacing_in_degrees) + 1;
 
     unsigned yaw_rdof, pitch_rdof, nsamples;
 };