optimize existing pose when it makes sense

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)