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/export.hpp | 28 -------------- cv/translation-calibrator.cpp | 41 ++++++++++++++++++++ cv/translation-calibrator.hpp | 36 +++++++++++++++++ cv/video-widget.cpp | 71 ++++++++++++++++++++++++++++++++++ cv/video-widget.hpp | 39 +++++++++++++++++++ tracker-aruco/ftnoir_tracker_aruco.cpp | 2 +- tracker-aruco/ftnoir_tracker_aruco.h | 15 +++---- tracker-aruco/pt_video_widget.cpp | 71 ---------------------------------- tracker-aruco/pt_video_widget.h | 39 ------------------- tracker-aruco/trans_calib.cpp | 41 -------------------- tracker-aruco/trans_calib.h | 36 ----------------- tracker-pt/ftnoir_tracker_pt.cpp | 4 +- tracker-pt/ftnoir_tracker_pt.h | 4 +- tracker-pt/ftnoir_tracker_pt_dialog.h | 4 +- tracker-pt/pt_video_widget.cpp | 71 ---------------------------------- tracker-pt/pt_video_widget.h | 39 ------------------- tracker-pt/trans_calib.cpp | 41 -------------------- tracker-pt/trans_calib.h | 36 ----------------- 18 files changed, 202 insertions(+), 416 deletions(-) delete mode 100644 cv/export.hpp create mode 100644 cv/translation-calibrator.cpp create mode 100644 cv/translation-calibrator.hpp create mode 100644 cv/video-widget.cpp create mode 100644 cv/video-widget.hpp delete mode 100644 tracker-aruco/pt_video_widget.cpp delete mode 100644 tracker-aruco/pt_video_widget.h delete mode 100644 tracker-aruco/trans_calib.cpp delete mode 100644 tracker-aruco/trans_calib.h delete mode 100644 tracker-pt/pt_video_widget.cpp delete mode 100644 tracker-pt/pt_video_widget.h delete mode 100644 tracker-pt/trans_calib.cpp delete mode 100644 tracker-pt/trans_calib.h diff --git a/cv/export.hpp b/cv/export.hpp deleted file mode 100644 index 6636e56b..00000000 --- a/cv/export.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#ifdef BUILD_cv -# ifdef _WIN32 -# define OPENTRACK_CV_LINKAGE __declspec(dllexport) -# else -# define OPENTRACK_CV_LINKAGE -# endif - -# ifndef _MSC_VER -# define OPENTRACK_CV_EXPORT __attribute__ ((visibility ("default"))) OPENTRACK_CV_LINKAGE -# else -# define OPENTRACK_CV_EXPORT OPENTRACK_CV_LINKAGE -# endif - -#else - #ifdef _WIN32 - # define OPENTRACK_CV_LINKAGE __declspec(dllimport) - #else - # define OPENTRACK_CV_LINKAGE - #endif - - #ifndef _MSC_VER - # define OPENTRACK_CV_EXPORT __attribute__ ((visibility ("default"))) OPENTRACK_CV_LINKAGE - #else - # define OPENTRACK_CV_EXPORT OPENTRACK_CV_LINKAGE - #endif -#endif 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]); +} diff --git a/cv/translation-calibrator.hpp b/cv/translation-calibrator.hpp new file mode 100644 index 00000000..cfde0051 --- /dev/null +++ b/cv/translation-calibrator.hpp @@ -0,0 +1,36 @@ +/* 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. + */ + +#pragma once + +#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::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k); + + // get the current estimate for t_MH + cv::Vec3f get_estimate(); + +private: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) +}; diff --git a/cv/video-widget.cpp b/cv/video-widget.cpp new file mode 100644 index 00000000..b514dc8f --- /dev/null +++ b/cv/video-widget.cpp @@ -0,0 +1,71 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * + * 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 "video-widget.hpp" +#include + +#include "opentrack/is-window-visible.hpp" + +cv_video_widget::cv_video_widget(QWidget* parent) : + QWidget(parent), + freshp(false), + visible(true) +{ + connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); + timer.start(50); +} + +void cv_video_widget::update_image(const cv::Mat& frame) +{ + QMutexLocker foo(&mtx); + + if (!freshp) + { + if (_frame.cols != frame.cols || _frame.rows != frame.rows) + { + _frame = cv::Mat(frame.rows, frame.cols, CV_8U); + _frame2 = cv::Mat(frame.rows, frame.cols, CV_8U); + } + frame.copyTo(_frame); + freshp = true; + } +} + +void cv_video_widget::paintEvent(QPaintEvent*) +{ + QMutexLocker foo(&mtx); + QPainter painter(this); + painter.drawImage(rect(), texture); +} + +void cv_video_widget::update_and_repaint() +{ + QMutexLocker l(&mtx); + + if (window_check_timer.elapsed_ms() > 250) + { + visible = is_window_visible(this); + window_check_timer.start(); + } + + if (visible) + { + if (_frame.empty() || !freshp) + return; + cv::cvtColor(_frame, _frame2, cv::COLOR_RGB2BGR); + + if (_frame3.cols != width() || _frame3.rows != height()) + _frame3 = cv::Mat(height(), width(), CV_8U); + + cv::resize(_frame2, _frame3, cv::Size(width(), height()), 0, 0, cv::INTER_NEAREST); + + texture = QImage((const unsigned char*) _frame3.data, _frame3.cols, _frame3.rows, QImage::Format_RGB888); + freshp = false; + update(); + } +} diff --git a/cv/video-widget.hpp b/cv/video-widget.hpp new file mode 100644 index 00000000..2d5d673f --- /dev/null +++ b/cv/video-widget.hpp @@ -0,0 +1,39 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * + * 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. + */ + +#pragma once + +#include "opentrack-compat/timer.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class cv_video_widget final : public QWidget +{ + Q_OBJECT +public: + cv_video_widget(QWidget *parent); + void update_image(const cv::Mat &frame); +protected slots: + void paintEvent(QPaintEvent*) override; + void update_and_repaint(); +private: + QMutex mtx; + QImage texture; + QTimer timer; + Timer window_check_timer; + cv::Mat _frame, _frame2, _frame3; + bool freshp, visible; +}; diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index d17ba599..02ca9920 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -73,7 +73,7 @@ Tracker::~Tracker() void Tracker::start_tracker(QFrame* videoframe) { videoframe->show(); - videoWidget = new PTVideoWidget(videoframe); + videoWidget = new cv_video_widget(videoframe); QHBoxLayout* layout_ = new QHBoxLayout(); layout_->setContentsMargins(0, 0, 0, 0); layout_->addWidget(videoWidget); diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h index f89f3fdc..6c28c7db 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.h +++ b/tracker-aruco/ftnoir_tracker_aruco.h @@ -8,13 +8,15 @@ #pragma once #include "ui_aruco-trackercontrols.h" -#include "pt_video_widget.h" -#include "opentrack-compat/options.hpp" -#include "trans_calib.h" #include "opentrack/plugin-api.hpp" -#include "cv/camera-dialog.hpp" #include "include/markerdetector.h" +#include "cv/camera-dialog.hpp" +#include "cv/video-widget.hpp" +#include "cv/translation-calibrator.hpp" + +#include + #include #include #include @@ -24,8 +26,7 @@ #include -#include - +#include "opentrack-compat/options.hpp" using namespace options; struct settings : opts { @@ -78,7 +79,7 @@ private: QMutex mtx; volatile bool stop; QHBoxLayout* layout; - PTVideoWidget* videoWidget; + cv_video_widget* videoWidget; settings s; double pose[6]; cv::Mat frame, grayscale, color; diff --git a/tracker-aruco/pt_video_widget.cpp b/tracker-aruco/pt_video_widget.cpp deleted file mode 100644 index 54bc9acd..00000000 --- a/tracker-aruco/pt_video_widget.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * - * 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 "pt_video_widget.h" -#include - -#include "opentrack/is-window-visible.hpp" - -PTVideoWidget::PTVideoWidget(QWidget* parent) : - QWidget(parent), - freshp(false), - visible(true) -{ - connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); - timer.start(50); -} - -void PTVideoWidget::update_image(const cv::Mat& frame) -{ - QMutexLocker foo(&mtx); - - if (!freshp) - { - if (_frame.cols != frame.cols || _frame.rows != frame.rows) - { - _frame = cv::Mat(frame.rows, frame.cols, CV_8U); - _frame2 = cv::Mat(frame.rows, frame.cols, CV_8U); - } - frame.copyTo(_frame); - freshp = true; - } -} - -void PTVideoWidget::paintEvent(QPaintEvent*) -{ - QMutexLocker foo(&mtx); - QPainter painter(this); - painter.drawImage(rect(), texture); -} - -void PTVideoWidget::update_and_repaint() -{ - QMutexLocker l(&mtx); - - if (window_check_timer.elapsed_ms() > 250) - { - visible = is_window_visible(this); - window_check_timer.start(); - } - - if (visible) - { - if (_frame.empty() || !freshp) - return; - cv::cvtColor(_frame, _frame2, cv::COLOR_RGB2BGR); - - if (_frame3.cols != width() || _frame3.rows != height()) - _frame3 = cv::Mat(height(), width(), CV_8U); - - cv::resize(_frame2, _frame3, cv::Size(width(), height()), 0, 0, cv::INTER_NEAREST); - - texture = QImage((const unsigned char*) _frame3.data, _frame3.cols, _frame3.rows, QImage::Format_RGB888); - freshp = false; - update(); - } -} diff --git a/tracker-aruco/pt_video_widget.h b/tracker-aruco/pt_video_widget.h deleted file mode 100644 index 82de3eab..00000000 --- a/tracker-aruco/pt_video_widget.h +++ /dev/null @@ -1,39 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * - * 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. - */ - -#pragma once - -#include "opentrack-compat/timer.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class PTVideoWidget final : public QWidget -{ - Q_OBJECT -public: - PTVideoWidget(QWidget *parent); - void update_image(const cv::Mat &frame); -protected slots: - void paintEvent(QPaintEvent*) override; - void update_and_repaint(); -private: - QMutex mtx; - QImage texture; - QTimer timer; - Timer window_check_timer; - cv::Mat _frame, _frame2, _frame3; - bool freshp, visible; -}; diff --git a/tracker-aruco/trans_calib.cpp b/tracker-aruco/trans_calib.cpp deleted file mode 100644 index b5148efd..00000000 --- a/tracker-aruco/trans_calib.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* 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]); -} diff --git a/tracker-aruco/trans_calib.h b/tracker-aruco/trans_calib.h deleted file mode 100644 index cfde0051..00000000 --- a/tracker-aruco/trans_calib.h +++ /dev/null @@ -1,36 +0,0 @@ -/* 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. - */ - -#pragma once - -#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::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k); - - // get the current estimate for t_MH - cv::Vec3f get_estimate(); - -private: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) -}; diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 49159ac8..429a0305 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -89,7 +89,7 @@ void Tracker_PT::run() while((commands & ABORT) == 0) { - const double dt = time.elapsed() * 1e-9; + const double dt = time.elapsed_seconds(); time.start(); bool new_frame; @@ -204,7 +204,7 @@ void Tracker_PT::start_tracker(QFrame *parent_window) video_frame = parent_window; video_frame->setAttribute(Qt::WA_NativeWindow); video_frame->show(); - video_widget = new PTVideoWidget(video_frame); + video_widget = new cv_video_widget(video_frame); QHBoxLayout* video_layout = new QHBoxLayout(parent_window); video_layout->setContentsMargins(0, 0, 0, 0); video_layout->addWidget(video_widget); diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h index 393f8e76..f515715e 100644 --- a/tracker-pt/ftnoir_tracker_pt.h +++ b/tracker-pt/ftnoir_tracker_pt.h @@ -14,9 +14,9 @@ #include "camera.h" #include "point_extractor.h" #include "point_tracker.h" -#include "pt_video_widget.h" #include "opentrack-compat/timer.hpp" #include "cv/camera-dialog.hpp" +#include "cv/video-widget.hpp" #include "opentrack-compat/pi-constant.hpp" #include @@ -68,7 +68,7 @@ private: PointExtractor point_extractor; PointTracker point_tracker; - PTVideoWidget* video_widget; + cv_video_widget* video_widget; QFrame* video_frame; settings_pt s; diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.h b/tracker-pt/ftnoir_tracker_pt_dialog.h index 87501b28..d2647a94 100644 --- a/tracker-pt/ftnoir_tracker_pt_dialog.h +++ b/tracker-pt/ftnoir_tracker_pt_dialog.h @@ -11,10 +11,10 @@ #include "opentrack/plugin-api.hpp" #include "ftnoir_tracker_pt_settings.h" #include "ftnoir_tracker_pt.h" -#include "trans_calib.h" -#include "pt_video_widget.h" #include "ui_FTNoIR_PT_Controls.h" #include "cv/camera-dialog.hpp" +#include "cv/translation-calibrator.hpp" +#include "cv/video-widget.hpp" #include diff --git a/tracker-pt/pt_video_widget.cpp b/tracker-pt/pt_video_widget.cpp deleted file mode 100644 index 54bc9acd..00000000 --- a/tracker-pt/pt_video_widget.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * - * 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 "pt_video_widget.h" -#include - -#include "opentrack/is-window-visible.hpp" - -PTVideoWidget::PTVideoWidget(QWidget* parent) : - QWidget(parent), - freshp(false), - visible(true) -{ - connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); - timer.start(50); -} - -void PTVideoWidget::update_image(const cv::Mat& frame) -{ - QMutexLocker foo(&mtx); - - if (!freshp) - { - if (_frame.cols != frame.cols || _frame.rows != frame.rows) - { - _frame = cv::Mat(frame.rows, frame.cols, CV_8U); - _frame2 = cv::Mat(frame.rows, frame.cols, CV_8U); - } - frame.copyTo(_frame); - freshp = true; - } -} - -void PTVideoWidget::paintEvent(QPaintEvent*) -{ - QMutexLocker foo(&mtx); - QPainter painter(this); - painter.drawImage(rect(), texture); -} - -void PTVideoWidget::update_and_repaint() -{ - QMutexLocker l(&mtx); - - if (window_check_timer.elapsed_ms() > 250) - { - visible = is_window_visible(this); - window_check_timer.start(); - } - - if (visible) - { - if (_frame.empty() || !freshp) - return; - cv::cvtColor(_frame, _frame2, cv::COLOR_RGB2BGR); - - if (_frame3.cols != width() || _frame3.rows != height()) - _frame3 = cv::Mat(height(), width(), CV_8U); - - cv::resize(_frame2, _frame3, cv::Size(width(), height()), 0, 0, cv::INTER_NEAREST); - - texture = QImage((const unsigned char*) _frame3.data, _frame3.cols, _frame3.rows, QImage::Format_RGB888); - freshp = false; - update(); - } -} diff --git a/tracker-pt/pt_video_widget.h b/tracker-pt/pt_video_widget.h deleted file mode 100644 index 82de3eab..00000000 --- a/tracker-pt/pt_video_widget.h +++ /dev/null @@ -1,39 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * Copyright (c) 2014-2016 Stanislaw Halik - * - * 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. - */ - -#pragma once - -#include "opentrack-compat/timer.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class PTVideoWidget final : public QWidget -{ - Q_OBJECT -public: - PTVideoWidget(QWidget *parent); - void update_image(const cv::Mat &frame); -protected slots: - void paintEvent(QPaintEvent*) override; - void update_and_repaint(); -private: - QMutex mtx; - QImage texture; - QTimer timer; - Timer window_check_timer; - cv::Mat _frame, _frame2, _frame3; - bool freshp, visible; -}; diff --git a/tracker-pt/trans_calib.cpp b/tracker-pt/trans_calib.cpp deleted file mode 100644 index a1a4b641..00000000 --- a/tracker-pt/trans_calib.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* 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::Matx33f& R_CM_k, const cv::Vec3f& 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]); -} diff --git a/tracker-pt/trans_calib.h b/tracker-pt/trans_calib.h deleted file mode 100644 index b697a7d4..00000000 --- a/tracker-pt/trans_calib.h +++ /dev/null @@ -1,36 +0,0 @@ -/* 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. - */ - -#pragma once - -#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 final -{ -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(); - -private: - cv::Matx66f P; // normalized precision matrix = inverse covariance - cv::Vec6f y; // P*(-t_MH, t_CH) -}; -- cgit v1.2.3