From 5a4c8f61e0ad001a8c462e50117b1fd474d27e6c Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 10 Aug 2016 09:45:00 +0200 Subject: cv: move calibrator and video widget to cv module Adjust usages in PT and Aruco trackers. --- cv/translation-calibrator.cpp | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 cv/translation-calibrator.cpp (limited to 'cv/translation-calibrator.cpp') diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp new file mode 100644 index 00000000..1287b408 --- /dev/null +++ b/cv/translation-calibrator.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 "translation-calibrator.hpp" + +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