From e7e8fd1c8b115dc9c2393fde29733eb301d4342c Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 27 Oct 2013 10:28:18 +0100 Subject: main: improve pose compensation Signed-off-by: Stanislaw Halik --- facetracknoir/tracker.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp index 01c16312..83f60356 100644 --- a/facetracknoir/tracker.cpp +++ b/facetracknoir/tracker.cpp @@ -128,9 +128,14 @@ void Tracker::run() { if (compensate) { - const auto H = output_camera.axes[Yaw] * M_PI / 180; - const auto P = output_camera.axes[Pitch] * M_PI / 180; - const auto B = output_camera.axes[Roll] * M_PI / 180; + const double yi = mainApp->axis(Yaw).invert; + const double pi = mainApp->axis(Pitch).invert; + const double ri = mainApp->axis(Roll).invert; + output_camera.axes[Yaw] += atan2(target_camera2.axes[TX] * yi, target_camera2.axes[TZ]); + output_camera.axes[Pitch] += atan2(target_camera2.axes[TY] * pi, target_camera2.axes[TZ]); + const auto H = output_camera.axes[Yaw] * M_PI / 180 * yi; + const auto P = output_camera.axes[Pitch] * M_PI / 180 * pi; + const auto B = output_camera.axes[Roll] * M_PI / 180 * ri; const auto cosH = cos(H); const auto sinH = sin(H); -- cgit v1.2.3