pt: calibrate only if pose received

Otherwise breaks center-on-startup logic.

2 files changed, 39 insertions, 32 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 23cc72c7..6b88884b 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -22,7 +22,8 @@ Tracker::Tracker()
     : mutex(QMutex::Recursive),
       commands(0),
 	  video_widget(NULL),
-	  video_frame(NULL)
+	  video_frame(NULL),
+      success(false)
 {
     connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings()));
 }
@@ -76,7 +77,9 @@ void Tracker::run()
 
             std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
             
-            if (points.size() == PointModel::N_POINTS)
+            success = points.size() == PointModel::N_POINTS;
+            
+            if (success)
                 point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose);
             
             {
@@ -158,36 +161,38 @@ void Tracker::StopTracker(bool exit)
 
 void Tracker::data(THeadPoseData *data)
 {
-
-    Affine X_CM = pose();
-
-    Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
-    Affine X_GH = X_CM * X_MH;
-
-    Matx33f R = X_GH.R * X_MH.R.t();
-    Vec3f   t = X_GH.t;
-
-    // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
-    // -z -> x, y -> z, x -> -y
-    Matx33f R_EG(0, 0,-1,
-                -1, 0, 0,
-                 0, 1, 0);
-    R = R_EG * R * R_EG.t();
-
-    // extract rotation angles
-    float alpha, beta, gamma;
-    beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
-    alpha = atan2( R(1,0), R(0,0));
-    gamma = atan2( R(2,1), R(2,2));
-
-    // extract rotation angles
-    data[Yaw] = rad2deg * alpha;
-    data[Pitch] = -rad2deg * beta;
-    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;
+    if (success)
+    {
+        Affine X_CM = pose();
+    
+        Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
+        Affine X_GH = X_CM * X_MH;
+    
+        Matx33f R = X_GH.R * X_MH.R.t();
+        Vec3f   t = X_GH.t;
+    
+        // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
+        // -z -> x, y -> z, x -> -y
+        Matx33f R_EG(0, 0,-1,
+                    -1, 0, 0,
+                     0, 1, 0);
+        R = R_EG * R * R_EG.t();
+    
+        // extract rotation angles
+        float alpha, beta, gamma;
+        beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+        alpha = atan2( R(1,0), R(0,0));
+        gamma = atan2( R(2,1), R(2,2));
+    
+        // extract rotation angles
+        data[Yaw] = rad2deg * alpha;
+        data[Pitch] = -rad2deg * beta;
+        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 14be5c7f..399d0693 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -71,6 +71,8 @@ private:
 
     settings s;
     Timer time;
+    
+    bool success;
 
     static constexpr double rad2deg = 180.0/3.14159265;
     static constexpr double deg2rad = 3.14159265/180.0;