diff options
Diffstat (limited to 'opentrack')
| -rw-r--r-- | opentrack/tracker.cpp | 38 |
1 files changed, 19 insertions, 19 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 2a93898c..c0169a14 100644 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -54,16 +54,16 @@ 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 { alpha, -beta, gamma }; + return cv::Vec3d { alpha, beta, gamma }; } // tait-bryan angles, not euler static cv::Matx33d euler_to_rmat(const double* input) { static constexpr double pi = 3.141592653; - const auto H = input[1] * pi / -180; - const auto P = input[2] * pi / -180; - const auto B = input[0] * pi / 180; + const auto H = input[0] * pi / 180; + const auto P = input[1] * pi / 180; + const auto B = input[2] * pi / 180; const auto c1 = cos(H); const auto s1 = sin(H); @@ -73,18 +73,18 @@ static cv::Matx33d euler_to_rmat(const double* input) const auto s3 = sin(B); double foo[] = { - // z - c1 * c3 - s1 * s2 * s3, - - s3 * c2, - s1 * c3 + c1 * s2 * s3, - // x - c1 * s3 + s1 * s2 * c3, - c3 * c2, - s3 * s1 - c1 * s2 * c3, - // y - - s1 * c2, - - s2, - c1 * c2, + // z + c1 * c2, + c1 * s2 * s3 - c3 * s1, + s1 * s3 + c1 * c3 * s2, + // y + c2 * s1, + c1 * c3 + s1 * s2 * s3, + c3 * s1 * s2 - c1 * s3, + // x + -s2, + c2 * s3, + c2 * c3 }; return cv::Matx33d(foo); @@ -92,11 +92,11 @@ 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[1], -xyz[0], xyz[2] }; + const double xyz_[3] = { xyz[0], xyz[1], xyz[2] }; cv::Matx31d tvec(xyz_); const cv::Matx31d ret = rmat * tvec; - output[0] = ret(1, 0); - output[1] = -ret(0, 0); + output[0] = ret(0, 0); + output[1] = ret(1, 0); if (!rz) output[2] = ret(2, 0); } |