From bef7aff31e5ea073f0f160ca6a2f1e56b7dd881a Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 15 Sep 2013 12:39:32 +0200 Subject: Initial PT 1.1 import Codebase broken at this stage --- FTNoIR_Tracker_PT/trans_calib.cpp | 44 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 FTNoIR_Tracker_PT/trans_calib.cpp (limited to 'FTNoIR_Tracker_PT/trans_calib.cpp') diff --git a/FTNoIR_Tracker_PT/trans_calib.cpp b/FTNoIR_Tracker_PT/trans_calib.cpp new file mode 100644 index 00000000..9b75a1b6 --- /dev/null +++ b/FTNoIR_Tracker_PT/trans_calib.cpp @@ -0,0 +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]); +} \ No newline at end of file -- cgit v1.2.3 From d6855de1a83fd54c43384de218d61252de7a7a1b Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 8 Jan 2014 23:53:45 +0100 Subject: fix crash due to mismatched float/double --- FTNoIR_Tracker_PT/point_tracker.cpp | 11 +++++++---- FTNoIR_Tracker_PT/point_tracker.h | 14 +++++++------- FTNoIR_Tracker_PT/trans_calib.cpp | 4 ++-- 3 files changed, 16 insertions(+), 13 deletions(-) (limited to 'FTNoIR_Tracker_PT/trans_calib.cpp') diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index 263be43a..9f0fb59e 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -105,8 +105,8 @@ void PointTracker::reset() dt_valid = 0; reset_velocities(); // assume identity rotation again - X_CM.R = cv::Matx33d::eye(); - X_CM.t = cv::Vec3f(); + X_CM.R = cv::Matx33f::eye(); + X_CM.t = cv::Vec3f(0, 0, 1); } void PointTracker::reset_velocities() @@ -168,7 +168,7 @@ bool PointTracker::track(const vector& points, float fov, float dt, int w void PointTracker::predict(float dt) { // predict with constant velocity - Matx33d R; + Matx33f R; Rodrigues(dt*v_r, R); X_CM.R = R*X_CM.R; X_CM.t += dt * v_t; @@ -273,7 +273,10 @@ void PointTracker::POSIT(float fov, int w, int h) cv::Rodrigues(rvec, rmat); // finally, find the closer solution - cv::Mat expected = cv::Mat(X_CM.R); + cv::Mat expected(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + expected.at(i, j) = X_CM.R(i, j); cv::Mat eye = cv::Mat::eye(3, 3, CV_64FC1); double dev1 = norm(eye - expected * rmat.t()); double dev2 = norm(eye - expected * rmat); diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index 823d75c0..ac43489e 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -21,11 +21,11 @@ class FrameTrafo { public: - FrameTrafo() : R(cv::Matx33d::eye()), t(0,0,0) {} - FrameTrafo(const cv::Matx33d& R, const cv::Vec3d& t) : R(R),t(t) {} + FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} + FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} - cv::Matx33d R; - cv::Vec3d t; + cv::Matx33f R; + cv::Vec3f t; }; inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) @@ -33,17 +33,17 @@ inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); } -inline FrameTrafo operator*(const cv::Matx33d& X, const FrameTrafo& Y) +inline FrameTrafo operator*(const cv::Matx33f& X, const FrameTrafo& Y) { return FrameTrafo(X*Y.R, X*Y.t); } -inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33d& Y) +inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33f& Y) { return FrameTrafo(X.R*Y, X.t); } -inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3d& v) +inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) { return X.R*v + X.t; } diff --git a/FTNoIR_Tracker_PT/trans_calib.cpp b/FTNoIR_Tracker_PT/trans_calib.cpp index 9b75a1b6..b2e0ead0 100644 --- a/FTNoIR_Tracker_PT/trans_calib.cpp +++ b/FTNoIR_Tracker_PT/trans_calib.cpp @@ -40,5 +40,5 @@ void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) Vec3f TranslationCalibrator::get_estimate() { Vec6f x = P.inv() * y; - return Vec3f(-x[0], -x[1], -x[2]); -} \ No newline at end of file + return Vec3f(-x[0], -x[1], -x[2]); +} -- cgit v1.2.3 From 80c43ba62517d1228423f9d1992999edda612616 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 12 Jan 2014 19:54:02 +0100 Subject: Revert "fix crash due to mismatched float/double" This reverts commit 9fc8e2da02f2e5c2295199826e408cc026d6eae6. --- FTNoIR_Tracker_PT/point_tracker.cpp | 11 ++++------- FTNoIR_Tracker_PT/point_tracker.h | 14 +++++++------- FTNoIR_Tracker_PT/trans_calib.cpp | 4 ++-- 3 files changed, 13 insertions(+), 16 deletions(-) (limited to 'FTNoIR_Tracker_PT/trans_calib.cpp') diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index 9f0fb59e..263be43a 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -105,8 +105,8 @@ void PointTracker::reset() dt_valid = 0; reset_velocities(); // assume identity rotation again - X_CM.R = cv::Matx33f::eye(); - X_CM.t = cv::Vec3f(0, 0, 1); + X_CM.R = cv::Matx33d::eye(); + X_CM.t = cv::Vec3f(); } void PointTracker::reset_velocities() @@ -168,7 +168,7 @@ bool PointTracker::track(const vector& points, float fov, float dt, int w void PointTracker::predict(float dt) { // predict with constant velocity - Matx33f R; + Matx33d R; Rodrigues(dt*v_r, R); X_CM.R = R*X_CM.R; X_CM.t += dt * v_t; @@ -273,10 +273,7 @@ void PointTracker::POSIT(float fov, int w, int h) cv::Rodrigues(rvec, rmat); // finally, find the closer solution - cv::Mat expected(3, 3, CV_64FC1); - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - expected.at(i, j) = X_CM.R(i, j); + cv::Mat expected = cv::Mat(X_CM.R); cv::Mat eye = cv::Mat::eye(3, 3, CV_64FC1); double dev1 = norm(eye - expected * rmat.t()); double dev2 = norm(eye - expected * rmat); diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index ac43489e..823d75c0 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -21,11 +21,11 @@ class FrameTrafo { public: - FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {} - FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {} + FrameTrafo() : R(cv::Matx33d::eye()), t(0,0,0) {} + FrameTrafo(const cv::Matx33d& R, const cv::Vec3d& t) : R(R),t(t) {} - cv::Matx33f R; - cv::Vec3f t; + cv::Matx33d R; + cv::Vec3d t; }; inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) @@ -33,17 +33,17 @@ inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y) return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t); } -inline FrameTrafo operator*(const cv::Matx33f& X, const FrameTrafo& Y) +inline FrameTrafo operator*(const cv::Matx33d& X, const FrameTrafo& Y) { return FrameTrafo(X*Y.R, X*Y.t); } -inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33f& Y) +inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33d& Y) { return FrameTrafo(X.R*Y, X.t); } -inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v) +inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3d& v) { return X.R*v + X.t; } diff --git a/FTNoIR_Tracker_PT/trans_calib.cpp b/FTNoIR_Tracker_PT/trans_calib.cpp index b2e0ead0..9b75a1b6 100644 --- a/FTNoIR_Tracker_PT/trans_calib.cpp +++ b/FTNoIR_Tracker_PT/trans_calib.cpp @@ -40,5 +40,5 @@ void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k) Vec3f TranslationCalibrator::get_estimate() { Vec6f x = P.inv() * y; - return Vec3f(-x[0], -x[1], -x[2]); -} + return Vec3f(-x[0], -x[1], -x[2]); +} \ No newline at end of file -- cgit v1.2.3