From 3a9e32e7c8c6df97720ba569ef64131b086ad281 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 12 Apr 2017 01:40:28 +0200 Subject: cv/calibrator: allow for experimental roll calibration It mostly works from my testing. --- cv/translation-calibrator.cpp | 58 +++++++++++++++++++++++---------- cv/translation-calibrator.hpp | 16 +++++---- tracker-aruco/ftnoir_tracker_aruco.cpp | 2 +- tracker-pt/ftnoir_tracker_pt_dialog.cpp | 2 +- 4 files changed, 52 insertions(+), 26 deletions(-) diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp index b1c47a8a..cdd573bc 100644 --- a/cv/translation-calibrator.cpp +++ b/cv/translation-calibrator.cpp @@ -13,9 +13,10 @@ constexpr double TranslationCalibrator::pitch_spacing_in_degrees; constexpr double TranslationCalibrator::yaw_spacing_in_degrees; +constexpr double TranslationCalibrator::roll_spacing_in_degrees; -TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof) : - yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof) +TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof, unsigned roll_rdof) : + yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof), roll_rdof(roll_rdof) { reset(); } @@ -25,7 +26,10 @@ void TranslationCalibrator::reset() P = cv::Matx66f::zeros(); y = cv::Vec6f(0,0,0, 0,0,0); - used_poses = std::vector(bin_count, false); + 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); + nsamples = 0; } @@ -56,6 +60,22 @@ std::tuple TranslationCalibrator::get_estimate() qDebug() << "calibrator:" << nsamples << "samples total"; + unsigned values[3] {}; + vec* in[] { &used_yaw_poses, &used_pitch_poses, &used_roll_poses }; + + for (unsigned k = 0; k < 3; k++) + { + const vec& data = *in[k]; + for (unsigned i = 0; i < data.size(); i++) + if (data[i]) + values[k]++; + } + + qDebug() << "samples" + << "yaw" << values[0] + << "pitch" << values[1] + << "roll" << values[2]; + return std::make_tuple(cv::Vec3f(-x[0], -x[1], -x[2]), nsamples); } @@ -71,24 +91,28 @@ 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/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; + 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); + + if (yaw_k < used_yaw_poses.size() && + pitch_k < used_pitch_poses.size() && + roll_k < used_roll_poses.size()) - if (idx >= 0 && idx < bin_count) { - if (used_poses[idx]) - { - return false; - } - else - { - used_poses[idx] = true; - return true; - } + 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; } else - qDebug() << "calibrator: index out of range" << "yaw" << yaw << "pitch" << pitch << "max" << bin_count; + qDebug() << "calibrator: index out of range" + << "yaw" << yaw_k + << "pitch" << pitch_k + << "roll" << roll_k; return false; } diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp index 2bf73839..40ba19e9 100644 --- a/cv/translation-calibrator.hpp +++ b/cv/translation-calibrator.hpp @@ -20,7 +20,7 @@ class TranslationCalibrator { public: - TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof); + TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof, unsigned roll_rdof); // reset the calibration process void reset(); @@ -33,18 +33,20 @@ public: private: bool check_bucket(const cv::Matx33d& R_CM_k); + static int get_index(int yaw, int pitch, int roll); 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 used_poses; + using vec = std::vector; + + vec used_yaw_poses; + vec used_pitch_poses; + vec used_roll_poses; static constexpr double yaw_spacing_in_degrees = 2.5; static constexpr double pitch_spacing_in_degrees = 1.5; + static constexpr double roll_spacing_in_degrees = 3.5; - // this allows allows us up to +-180 for yaw and pitch - static constexpr int bin_count = 361*360 / (yaw_spacing_in_degrees*pitch_spacing_in_degrees) + 1; - - unsigned yaw_rdof, pitch_rdof, nsamples; + unsigned yaw_rdof, pitch_rdof, roll_rdof, nsamples; }; diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index b69439c6..edb6ffcc 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -453,7 +453,7 @@ void aruco_tracker::data(double *data) } aruco_dialog::aruco_dialog() : - calibrator(1, 0) + calibrator(1, 0, 2) { tracker = nullptr; calib_timer.setInterval(100); diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.cpp b/tracker-pt/ftnoir_tracker_pt_dialog.cpp index b47dac26..98e8e424 100644 --- a/tracker-pt/ftnoir_tracker_pt_dialog.cpp +++ b/tracker-pt/ftnoir_tracker_pt_dialog.cpp @@ -17,7 +17,7 @@ TrackerDialog_PT::TrackerDialog_PT() : tracker(nullptr), timer(this), - trans_calib(1, 2) + trans_calib(1, 2, 0) { ui.setupUi(this); -- cgit v1.2.3