From 3831edae9a45d65f6636767d6f6c53378e036b06 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 25 Oct 2013 14:08:56 +0200
Subject: add translation compensation

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
---
 facetracknoir/curve-config.cpp  | 12 ++++++++++++
 facetracknoir/facetracknoir.cpp |  3 +++
 facetracknoir/ftnoir_curves.ui  | 25 ++++++++++++++++++++++---
 facetracknoir/tracker.cpp       | 41 +++++++++++++++++++++++++++++++++++++++--
 facetracknoir/tracker.h         |  1 +
 5 files changed, 77 insertions(+), 5 deletions(-)

diff --git a/facetracknoir/curve-config.cpp b/facetracknoir/curve-config.cpp
index 50aa8acf..6da1730d 100644
--- a/facetracknoir/curve-config.cpp
+++ b/facetracknoir/curve-config.cpp
@@ -1,6 +1,7 @@
 #include "facetracknoir/facetracknoir.h"
 #include "facetracknoir/curve-config.h"
 #include <QDebug>
+#include <QCheckBox>
 CurveConfigurationDialog::CurveConfigurationDialog(FaceTrackNoIR *ftnoir, QWidget *parent) :
     QWidget( parent, Qt::Dialog ), mainApp(ftnoir)
 {
@@ -93,6 +94,8 @@ void CurveConfigurationDialog::loadSettings() {
 
     iniFile.beginGroup("Tracking");
 
+    ui.checkBox->setChecked(iniFile.value("compensation", true).toBool());
+
     for (int i = 0; i < 6; i++)
         mainApp->axis(i).altp = iniFile.value(names[i], false).toBool();
 
@@ -179,6 +182,8 @@ void CurveConfigurationDialog::loadSettings() {
         connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int)), Qt::UniqueConnection);
         mainApp->axis(i).zero = widgets3[i]->value();
     }
+
+    connect(ui.checkBox, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int)));
     
     settingsDirty = false;
 }
@@ -212,6 +217,13 @@ void CurveConfigurationDialog::save() {
 
     iniFile.beginGroup("Tracking");
 
+    bool compensate = true;
+
+    iniFile.setValue("compensation", compensate = (bool) !!ui.checkBox->isChecked());
+
+    if (mainApp->tracker)
+        mainApp->tracker->compensate = compensate;
+
     iniFile.setValue("rx_alt", ui.rx_altp->checkState() != Qt::Unchecked);
     iniFile.setValue("ry_alt", ui.ry_altp->checkState() != Qt::Unchecked);
     iniFile.setValue("rz_alt", ui.rz_altp->checkState() != Qt::Unchecked);
diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp
index 8028d0ca..73daca70 100644
--- a/facetracknoir/facetracknoir.cpp
+++ b/facetracknoir/facetracknoir.cpp
@@ -257,6 +257,7 @@ void FaceTrackNoIR::save() {
 	QSettings iniFile( currentFile, QSettings::IniFormat );	
 
 	iniFile.beginGroup ( "Tracking" );
+
 	iniFile.setValue ( "invertYaw", ui.chkInvertYaw->isChecked() );
 	iniFile.setValue ( "invertPitch", ui.chkInvertPitch->isChecked() );
 	iniFile.setValue ( "invertRoll", ui.chkInvertRoll->isChecked() );
@@ -489,6 +490,8 @@ void FaceTrackNoIR::startTracker( ) {
 
 	tracker = new Tracker ( this );
 
+    tracker->compensate = iniFile.value("compensate", true).toBool();
+
     tracker->setInvertAxis(Yaw, ui.chkInvertYaw->isChecked() );
     tracker->setInvertAxis(Pitch, ui.chkInvertPitch->isChecked() );
     tracker->setInvertAxis(Roll, ui.chkInvertRoll->isChecked() );
diff --git a/facetracknoir/ftnoir_curves.ui b/facetracknoir/ftnoir_curves.ui
index d0c1b2cd..0c269c70 100644
--- a/facetracknoir/ftnoir_curves.ui
+++ b/facetracknoir/ftnoir_curves.ui
@@ -99,7 +99,7 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>984</width>
+     <width>971</width>
      <height>621</height>
     </rect>
    </property>
@@ -110,7 +110,7 @@
     <enum>QTabWidget::North</enum>
    </property>
    <property name="currentIndex">
-    <number>0</number>
+    <number>6</number>
    </property>
    <widget class="QWidget" name="tabWidgetPage1">
     <attribute name="title">
@@ -648,7 +648,7 @@
    </widget>
    <widget class="QWidget" name="tabWidgetPage7">
     <attribute name="title">
-     <string>Defaults</string>
+     <string>Options</string>
     </attribute>
     <widget class="QLabel" name="label">
      <property name="geometry">
@@ -860,6 +860,25 @@
       <string>TZ</string>
      </property>
     </widget>
+    <widget class="QCheckBox" name="checkBox">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>100</y>
+       <width>192</width>
+       <height>21</height>
+      </rect>
+     </property>
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
+     <property name="text">
+      <string>Translation compensation</string>
+     </property>
+    </widget>
    </widget>
   </widget>
  </widget>
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 980415c8..ac9de5cb 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -23,16 +23,18 @@
 *********************************************************************************/
 #include "tracker.h"
 #include "facetracknoir.h"
+#include <opencv2/core/core.hpp>
+#include <cmath>
 
 #if defined(_WIN32)
 #   include <windows.h>
 #endif
 
-/** constructor **/
 Tracker::Tracker( FaceTrackNoIR *parent ) :
     should_quit(false),
     do_center(false),
-    enabled(true)
+    enabled(true),
+    compensate(true)
 {
     // Retieve the pointer to the parent
     mainApp = parent;
@@ -124,6 +126,41 @@ void Tracker::run() {
                 get_curve(new_camera.axes[i], output_camera.axes[i], mainApp->axis(i));
             }
 
+            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 auto cosH = cos(H);
+                const auto sinH = sin(H);
+                const auto cosP = cos(P);
+                const auto sinP = sin(P);
+                const auto cosB = cos(B);
+                const auto sinB = sin(B);
+
+                double foo[] = {
+                    cosH * cosB - sinH * sinP * sinB,
+                    - sinB * cosP,
+                    sinH * cosB + cosH * sinP * sinB,
+                    cosH * sinB + sinH * sinP * cosB,
+                    cosB * cosP,
+                    sinB * sinH - cosH * sinP * cosB,
+                    - sinH * cosP,
+                    - sinP,
+                    cosH * cosP,
+                };
+
+                cv::Mat rmat(3, 3, CV_64F, foo);
+                cv::Mat tvec(3, 1, CV_64F, output_camera.axes);
+                cv::Mat ret = rmat * tvec;
+
+                cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1);
+
+                for (int i = 0; i < 3; i++)
+                    output_camera.axes[i] = ret.at<double>(i);
+            }
+
             if (Libraries->pProtocol) {
                 gameoutput_camera = output_camera;
                 Libraries->pProtocol->sendHeadposeToGame( gameoutput_camera.axes );	// degrees & centimeters
diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h
index 24a22c16..a7951ceb 100644
--- a/facetracknoir/tracker.h
+++ b/facetracknoir/tracker.h
@@ -109,6 +109,7 @@ public:
     // following are now protected by hTrackMutex
     volatile bool do_center;							// Center head-position, using the shortkey
     volatile bool enabled;
+    volatile bool compensate;
     
     T6DOF output_camera;
 };
-- 
cgit v1.2.3