summaryrefslogtreecommitdiffhomepage
path: root/opentrack
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-25 21:22:07 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-25 21:22:07 +0200
commitcbabfdd99bc892311c0bee4143aa7242400d39ce (patch)
tree55deb1260c85d159be837febe746234be24578ff /opentrack
parentd1f1067007524f68e3e6dec074d21e1884722507 (diff)
Change rot order tait-bryan -> mat
Issue: 63
Diffstat (limited to 'opentrack')
-rw-r--r--opentrack/tracker.cpp38
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);
}