diff options
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 9 |
1 files changed, 6 insertions, 3 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 67c88e3e..bb4ca96f 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -196,7 +196,9 @@ void Tracker::run() int cur_fps = 0; int last_fps = 0; cv::Point2f last_centroid; - bool first = true, pitch_sign = true; + const double pitch_eps = 60; + double last_pitch = 0; + bool first = true; while (!stop) { @@ -333,10 +335,11 @@ void Tracker::run() { cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); - if ((euler[0] > 0) != pitch_sign) + if (fabs(euler[0] - last_pitch) > pitch_eps) { first = true; - pitch_sign = euler[0] > 0; + last_pitch = euler[0]; + qDebug() << "reset levmarq due to pitch breakage"; } QMutexLocker lck(&mtx); |