diff options
Diffstat (limited to 'cv')
| -rw-r--r-- | cv/translation-calibrator.cpp | 17 | ||||
| -rw-r--r-- | cv/translation-calibrator.hpp | 7 |
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; }; |