From aa066bdd4622d4f6824fee864f6be6806813f04d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 30 Oct 2015 07:37:41 +0100 Subject: move to subdirectory-based build system Closes #224 --- tracker-aruco/trans_calib.cpp | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 tracker-aruco/trans_calib.cpp (limited to 'tracker-aruco/trans_calib.cpp') diff --git a/tracker-aruco/trans_calib.cpp b/tracker-aruco/trans_calib.cpp new file mode 100644 index 00000000..b5148efd --- /dev/null +++ b/tracker-aruco/trans_calib.cpp @@ -0,0 +1,41 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "trans_calib.h" + +TranslationCalibrator::TranslationCalibrator() +{ + reset(); +} + +void TranslationCalibrator::reset() +{ + P = cv::Matx66f::zeros(); + y = cv::Vec6f(0,0,0, 0,0,0); +} + +void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k) +{ + cv::Matx H_k_T = cv::Matx::zeros(); + for (int i=0; i<3; ++i) { + for (int j=0; j<3; ++j) { + H_k_T(i,j) = R_CM_k(j,i); + } + } + for (int i=0; i<3; ++i) + { + H_k_T(3+i,i) = 1.0; + } + P += H_k_T * H_k_T.t(); + y += H_k_T * t_CM_k; +} + +cv::Vec3f TranslationCalibrator::get_estimate() +{ + cv::Vec6f x = P.inv() * y; + return cv::Vec3f(x[0], x[1], x[2]); +} -- cgit v1.2.3