add translation compensation

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

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;
 };