diff options
Diffstat (limited to 'tracker-pt')
| -rw-r--r-- | tracker-pt/ftnoir_tracker_pt.cpp | 18 |
1 files changed, 15 insertions, 3 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 2a13e3c6..ef6058db 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -235,6 +235,12 @@ void Tracker_PT::data(double *data) using std::atan2; using std::sqrt; + using std::atan; + using std::fabs; + using std::copysign; + + // get translation(s) + const vec3& t = X_GH.t; // extract rotation angles { @@ -243,13 +249,19 @@ void Tracker_PT::data(double *data) alpha = atan2( R(1,0), R(0,0)); gamma = atan2( R(2,1), R(2,2)); +#if 0 + if (t[2] > 1e-4) + { + alpha += copysign(atan(t[0] / t[2]), t[0]); + // pitch is skewed anyway due to only one focal length value + //beta -= copysign(atan(t[1] / t[2]), t[1]); + } +#endif + data[Yaw] = rad2deg * alpha; data[Pitch] = -rad2deg * beta; data[Roll] = rad2deg * gamma; } - // get translation(s) - - const vec3& t = X_GH.t; // convert to cm data[TX] = t[0] / 10; |