2 files changed, 16 insertions, 34 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index a32288f4..91f94c71 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -57,9 +57,10 @@ void Tracker::run()
 #endif
 
     while((commands & ABORT) == 0)
-	{   
+    {
         apply_inner();
         const double dt = time.start() * 1e-9;
+        cv::Mat frame;
         const bool new_frame = camera.get_frame(dt, &frame);
 
         if (new_frame && !frame.empty())
@@ -91,14 +92,13 @@ void Tracker::run()
         if (!frame.empty()) log_stream<<" fps: "<<camera.get_info().fps;
         log_stream<<"\n";
 #endif
-	}
-
-	qDebug()<<"Tracker:: Thread stopping";
+    }
+    qDebug()<<"Tracker:: Thread stopping";
 }
 void Tracker::apply(settings& s)
 {
     // caller guarantees object lifetime
-	new_settings = &s;
+    new_settings = &s;
 }
 
 void Tracker::apply_inner()
@@ -109,7 +109,7 @@ void Tracker::apply_inner()
     reset();
     auto& s = *tmp;
     qDebug()<<"Tracker:: Applying settings";
-    
+
     {
         cv::Vec3f M01(s.m01_x, s.m01_y, s.m01_z);
         cv::Vec3f M02(s.m02_x, s.m02_y, s.m02_z);
@@ -123,9 +123,7 @@ void Tracker::apply_inner()
     point_extractor.min_size = s.min_point_size;
     point_extractor.max_size = s.max_point_size;
     t_MH = cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z);
-    R_GC = cv::Matx33f::eye();
     FrameTrafo X_MH(Matx33f::eye(), t_MH);
-    X_GH_0 = R_GC * X_MH;
     qDebug()<<"Tracker::apply ends";
 }
 
@@ -135,15 +133,6 @@ void Tracker::reset()
 	point_tracker.reset();
 }
 
-void Tracker::center()
-{
-	point_tracker.reset();
-	QMutexLocker lock(&mutex);
-	FrameTrafo X_CM_0 = point_tracker.pose();
-	FrameTrafo X_MH(Matx33f::eye(), t_MH);
-	X_GH_0 = R_GC * X_CM_0 * X_MH;
-}
-
 void Tracker::start_tracker(QFrame *parent_window)
 {
     this->video_frame = parent_window;
@@ -174,19 +163,12 @@ void Tracker::StopTracker(bool exit)
 
 void Tracker::data(THeadPoseData *data)
 {
-	{
-		QMutexLocker lock(&mutex);
 
     	FrameTrafo X_CM = point_tracker.pose();
 		FrameTrafo X_MH(Matx33f::eye(), t_MH);
-		FrameTrafo X_GH = R_GC * X_CM * X_MH;
-        Matx33f R = X_GH.R * X_GH_0.R.t();
-		Vec3f   t = X_GH.t - X_GH_0.t;		
-
-        // get translation(s)
-        data[TX] = t[0] / 10.0;	// convert to cm
-        data[TY] = t[1] / 10.0;
-        data[TZ] = t[2] / 10.0;
+		FrameTrafo X_GH = X_CM * X_MH;
+        Matx33f R = X_GH.R;
+		Vec3f   t = X_GH.t;
 
         // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
 		// -z -> x, y -> z, x -> -y
@@ -201,10 +183,16 @@ void Tracker::data(THeadPoseData *data)
 		alpha = atan2( R(1,0), R(0,0));
 		gamma = atan2( R(2,1), R(2,2));		
 
+        QMutexLocker lock(&mutex);
+
         data[Yaw]   =   rad2deg * alpha;
         data[Pitch] = - rad2deg * beta;	// FTNoIR expects a minus here
         data[Roll]  =   rad2deg * gamma;
-	}
+
+        // get translation(s)
+        data[TX] = t[0] / 10.0;	// convert to cm
+        data[TY] = t[1] / 10.0;
+        data[TZ] = t[2] / 10.0;
 }
 
 //-----------------------------------------------------------------------------
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 33e03ea8..f7520c45 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -43,7 +43,6 @@ public:
 
     void apply(settings& s);
     void apply_inner();
-    void center();
     void reset();	// reset the trackers internal state variables
 
     void pose(FrameTrafo* X_CM) { QMutexLocker lock(&mutex); *X_CM = point_tracker.pose(); }
@@ -65,12 +64,7 @@ private:
     PointExtractor point_extractor;
     PointTracker   point_tracker;
 
-    FrameTrafo X_GH_0; // for centering
     cv::Vec3f   t_MH; // translation from model frame to head frame
-    cv::Matx33f R_GC; // rotation from opengl reference frame to camera frame
-
-    // --- ui ---
-    cv::Mat frame;	// the output frame for display
 
     PTVideoWidget* video_widget;
     QFrame*      video_frame;