pose breaks with negative pitch

2 files changed, 3 insertions, 3 deletions

diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index 8b508897..dfefdaf8 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -267,8 +267,8 @@ int PointTracker::POSIT(float f)
 	

 	// initial pose = last (predicted) pose

 	Vec3f k;

-	get_row(X_CM.R, 2, k);

-	float Z0 = X_CM.t[2];

+    get_row(R_expected, 2, k);

+    float Z0 = init_phase ? 1000 : X_CM.t[2];

 

 	float old_epsilon_1 = 0;

 	float old_epsilon_2 = 0;

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 0719faac..93b43de7 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -335,7 +335,7 @@ void Tracker::run()
             {
                 cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
 
-                if (fabs(euler[0] - last_pitch) > pitch_eps)
+                if (fabs(euler[0] - last_pitch) > pitch_eps || euler[0] < 0)
                 {
                     first = true;
                     last_pitch = euler[0];