From 4b21008a53f2d89a0143976305368584d4ed1e44 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 24 Sep 2014 19:38:08 +0200 Subject: CRLF -> LF conversion .git/index removal Forgot to do it earlier, hence trash commits in recent days --- ftnoir_tracker_aruco/trans_calib.cpp | 88 ++++++++++++++++++------------------ ftnoir_tracker_aruco/trans_calib.h | 76 +++++++++++++++---------------- 2 files changed, 82 insertions(+), 82 deletions(-) (limited to 'ftnoir_tracker_aruco') diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp index dd18399a..b1f956b4 100644 --- a/ftnoir_tracker_aruco/trans_calib.cpp +++ b/ftnoir_tracker_aruco/trans_calib.cpp @@ -1,44 +1,44 @@ -/* 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" - -using namespace cv; - -//----------------------------------------------------------------------------- -TranslationCalibrator::TranslationCalibrator() -{ - reset(); -} - -void TranslationCalibrator::reset() -{ - P = Matx66f::zeros(); - y = Vec6f(0,0,0, 0,0,0); -} - -void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) -{ - Matx H_k_T = 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; -} - -Vec3f TranslationCalibrator::get_estimate() -{ - Vec6f x = P.inv() * y; - return Vec3f(x[0], x[1], x[2]); -} +/* 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" + +using namespace cv; + +//----------------------------------------------------------------------------- +TranslationCalibrator::TranslationCalibrator() +{ + reset(); +} + +void TranslationCalibrator::reset() +{ + P = Matx66f::zeros(); + y = Vec6f(0,0,0, 0,0,0); +} + +void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) +{ + Matx H_k_T = 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; +} + +Vec3f TranslationCalibrator::get_estimate() +{ + Vec6f x = P.inv() * y; + return Vec3f(x[0], x[1], x[2]); +} diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h index f2521690..609c9af1 100644 --- a/ftnoir_tracker_aruco/trans_calib.h +++ b/ftnoir_tracker_aruco/trans_calib.h @@ -1,39 +1,39 @@ -/* 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. - */ - -#ifndef TRANSCALIB_H -#define TRANSCALIB_H - -#include - -//----------------------------------------------------------------------------- -// Calibrates the translation from head to model = t_MH -// by recursive least squares / -// kalman filter in information form with identity noise covariance -// measurement equation when head position = t_CH is fixed: -// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k - -class TranslationCalibrator -{ -public: - TranslationCalibrator(); - - // reset the calibration process - void reset(); - - // update the current estimate - void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); - - // get the current estimate for t_MH - cv::Vec3f get_estimate(); - -protected: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) -}; - +/* 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. + */ + +#ifndef TRANSCALIB_H +#define TRANSCALIB_H + +#include + +//----------------------------------------------------------------------------- +// Calibrates the translation from head to model = t_MH +// by recursive least squares / +// kalman filter in information form with identity noise covariance +// measurement equation when head position = t_CH is fixed: +// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k + +class TranslationCalibrator +{ +public: + TranslationCalibrator(); + + // reset the calibration process + void reset(); + + // update the current estimate + void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k); + + // get the current estimate for t_MH + cv::Vec3f get_estimate(); + +protected: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) +}; + #endif //TRANSCALIB_H \ No newline at end of file -- cgit v1.2.3