diff options
| -rw-r--r-- | FTNoIR_Tracker_PT/point_tracker.cpp | 4 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 2 |
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]; |