main: improve pose compensation

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

1 files 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);