summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2017-03-16 13:39:38 +0100
committerStanislaw Halik <sthalik@misaki.pl>2017-03-16 13:41:16 +0100
commit7aa0b084b97e66e0869d44b67ee02b7d45fbb661 (patch)
treeb5f03156e1b66a5b997c8761f562c34997693120
parent426ee39a2c2a50f08ed89b7471ba40416293e451 (diff)
cv/calibrator: limit samples at similar positions
Having yaw and pitch as a tuple, let N be the granularity. We're now only allowing one sample per the granularity level. Granularity -- "spacing_in_degrees" has a value of 3 degrees. For now the values must be integral. Since we're only allowing (yaw, pitch) tuples of given granularity, the following get treated as distinct: (0; 0), (0; 3), (0; 6), (1; 42), (3; 3) The tuple value order can be swapped. There's nothing significant as for what's pitch and what's yaw. We drop the remainder between the yaw/pitch value so (0, 0) is index 0, (0; N) is index 1, (0; 2N) index 3, etc. This should prevent the calibration function from biasing itself when the user keeps still during the procedure.
-rw-r--r--cv/translation-calibrator.cpp52
-rw-r--r--cv/translation-calibrator.hpp17
2 files changed, 66 insertions, 3 deletions
diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
index a5df5b5f..75526815 100644
--- a/cv/translation-calibrator.cpp
+++ b/cv/translation-calibrator.cpp
@@ -6,8 +6,13 @@
*/
#include "translation-calibrator.hpp"
+#include "compat/euler.hpp"
+#include "compat/util.hpp"
-TranslationCalibrator::TranslationCalibrator()
+#include <cmath>
+
+TranslationCalibrator::TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof) :
+ yaw_rdof(yaw_rdof), pitch_rdof(pitch_rdof)
{
reset();
}
@@ -16,10 +21,18 @@ void TranslationCalibrator::reset()
{
P = cv::Matx66f::zeros();
y = cv::Vec6f(0,0,0, 0,0,0);
+
+ used_poses = std::vector<bool>(bin_count, false);
+ nsamples = 0;
}
void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k)
{
+ if (!check_bucket(R_CM_k))
+ return;
+
+ nsamples++;
+
cv::Matx<double, 6,3> H_k_T = cv::Matx<double, 6,3>::zeros();
for (int i=0; i<3; ++i) {
for (int j=0; j<3; ++j) {
@@ -37,5 +50,42 @@ void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t
cv::Vec3f TranslationCalibrator::get_estimate()
{
cv::Vec6f x = P.inv() * y;
+
+ qDebug() << "calibrator:" << nsamples << "samples total";
+
return cv::Vec3f(-x[0], -x[1], -x[2]);
}
+
+bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
+{
+ const int idx = progn(
+ using namespace euler;
+ static constexpr double r2d = 180/M_PI;
+
+ rmat r;
+ for (unsigned j = 0; j < 3; j++)
+ for (unsigned i = 0; i < 3; i++)
+ r(j, i) = R_CM_k(j, i);
+
+ 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;
+ return pitch * 360/spacing_in_degrees + yaw;
+ );
+
+ if (idx >= 0 && idx < bin_count)
+ {
+ if (used_poses[idx])
+ {
+ return false;
+ }
+ else
+ {
+ used_poses[idx] = true;
+ return true;
+ }
+ }
+
+ return false;
+}
diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp
index cfde0051..76fb5db9 100644
--- a/cv/translation-calibrator.hpp
+++ b/cv/translation-calibrator.hpp
@@ -7,7 +7,8 @@
#pragma once
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
+#include <vector>
//-----------------------------------------------------------------------------
// Calibrates the translation from head to model = t_MH
@@ -19,7 +20,7 @@
class TranslationCalibrator
{
public:
- TranslationCalibrator();
+ TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof);
// reset the calibration process
void reset();
@@ -31,6 +32,18 @@ public:
cv::Vec3f get_estimate();
private:
+ 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)
+
+ // 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;
+
+ // 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;
+
+ unsigned yaw_rdof, pitch_rdof, nsamples;
};