summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp')
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp9
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);