diff options
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 12 |
1 files changed, 11 insertions, 1 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 70c82c52..9c8cd52b 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -195,6 +195,7 @@ void Tracker::run() int cur_fps = 0; int last_fps = 0; cv::Point2f last_centroid; + bool first = true, pitch_sign = true; while (!stop) { @@ -318,7 +319,9 @@ void Tracker::run() last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height); } - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE); + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE); + + first = false; cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1); @@ -329,6 +332,12 @@ void Tracker::run() { cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); + if ((euler[0] > 0) != pitch_sign) + { + first = true; + pitch_sign = euler[0] > 0; + } + QMutexLocker lck(&mtx); for (int i = 0; i < 3; i++) @@ -354,6 +363,7 @@ void Tracker::run() else { last_roi = cv::Rect(65535, 65535, 0, 0); + first = true; } if (frame.rows > 0) |