summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-01-06 23:51:31 +0100
committerStanislaw Halik <sthalik@misaki.pl>2014-01-06 23:51:31 +0100
commitf5496ba4712f16244b8ab8dc90253e19ef1ad54f (patch)
treeda8821d2fab0405a7d227a874e267a372e7db5a6
parent2a7bb33871dee1124dd1956fe14503845e8cee3f (diff)
treat pitch delta +- 60 as breakage, instead of sign change
-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);