From cc4e7e331ba24b260edc44db8b09de274cd658a7 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 7 Jan 2014 09:00:57 +0100 Subject: pose breaks with negative pitch --- FTNoIR_Tracker_PT/point_tracker.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'FTNoIR_Tracker_PT') 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; -- cgit v1.2.3