From ea0eb13a5b37811c0a597c78241401e52bb4972e Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 29 Oct 2014 05:41:37 +0100 Subject: r/t compensation broken, fix --- opentrack/tracker.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 7a6f908a..3d5f6cbd 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -56,14 +56,14 @@ static cv::Vec3d rmat_to_euler(const cv::Matx33d& R) const double beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) ); const double alpha = atan2( R(1,0), R(0,0)); const double gamma = atan2( R(2,1), R(2,2)); - return cv::Vec3d { gamma, alpha, beta }; + return cv::Vec3d { gamma, -alpha, beta }; } // tait-bryan angles, not euler static cv::Matx33d euler_to_rmat(const double* input) { static constexpr double pi = 3.141592653; - const auto H = input[x] * pi / 180; + const auto H = input[x] * -pi / 180; const auto P = input[y] * pi / 180; const auto B = input[z] * pi / 180; @@ -86,13 +86,14 @@ static cv::Matx33d euler_to_rmat(const double* input) void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* output, bool rz) { - const double xyz_[3] = { xyz[x], -xyz[y], xyz[z] }; + // same order/sign as rmat_to_euler retval + const double xyz_[3] = { xyz[2], -xyz[0], xyz[1] }; cv::Matx31d tvec(xyz_); const cv::Matx31d ret = rmat * tvec; if (!rz) - output[2] = ret(z, 0); - output[1] = -ret(y, 0); - output[0] = ret(x, 0); + output[2] = ret(0, 0); + output[1] = ret(0, 0); + output[0] = -ret(1, 0); } void Tracker::logic() -- cgit v1.2.3