tracker/aruco: fix #375

The height was zero on the test video. The ROI check only saw if width
is at least zero. Check for both to be greater than one.

Video provided by: @kblomster
Issue: #375

Also, fix minor issues:

- nix vars that can be const static in function scope
- don't call solvepnp twice where obj_points shift will do
- don't do bounds checking on vector elt access
- respect sprintf varargs type size; change to snprintf for no reason
- fix clamp-to-image logic
- set proper alpha for fps estimation

1 files changed, 33 insertions, 29 deletions

diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp
index 65056a63..d9579743 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.cpp
+++ b/tracker-aruco/ftnoir_tracker_aruco.cpp
@@ -43,7 +43,6 @@ Tracker::Tracker() :
     obj_points(4),
     intrinsics(decltype(intrinsics)::eye()),
     dist_coeffs(decltype(dist_coeffs)::zeros()),
-    centroid { cv::Point3f(0, 0, 0) },
     rmat(decltype(rmat)::eye()),
     roi_points(4),
     last_roi(65535, 65535, 0, 0),
@@ -98,25 +97,29 @@ void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
 
 bool Tracker::detect_with_roi()
 {
-    if (last_roi.width > 0)
+    if (last_roi.width > 1 && last_roi.height > 1)
     {
-        detector.setMinMaxSize(std::min(1.f, std::max(0.01f, size_min * grayscale.cols / last_roi.width)),
-                               std::max(.01f, std::min(1.0f, size_max * grayscale.cols / last_roi.width)));
+        float min = std::min(1.f, std::max(.01f, size_min * grayscale.cols / last_roi.width));
+        float max = std::max(.01f, std::min(1.f, size_max * grayscale.cols / last_roi.width));
+        detector.setMinMaxSize(min, max);
 
-        detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false);
+        cv::Mat grayscale_ = grayscale(last_roi);
+
+        detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false);
 
         if (markers.size() == 1 && markers[0].size() == 4)
         {
-            auto& m = markers.at(0);
+            auto& m = markers[0];
             for (unsigned i = 0; i < 4; i++)
             {
-                auto& p = m.at(i);
+                auto& p = m[i];
                 p.x += last_roi.x;
                 p.y += last_roi.y;
             }
             return true;
         }
     }
+
     last_roi = cv::Rect(65535, 65535, 0, 0);
     return false;
 }
@@ -203,13 +206,14 @@ void Tracker::draw_ar(bool ok)
 {
     if (ok)
     {
-        const auto& m = markers.at(0);
+        const auto& m = markers[0];
         for (unsigned i = 0; i < 4; i++)
             cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8);
     }
 
     char buf[32];
-    ::sprintf(buf, "Hz: %d", (unsigned short)cur_fps);
+    ::snprintf(buf, sizeof(buf)-1, "Hz: %d", (int)(unsigned short)cur_fps);
+    buf[sizeof(buf)-1] = '\0';
     cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1);
 }
 
@@ -219,10 +223,10 @@ void Tracker::clamp_last_roi()
         last_roi.x = 0;
     if (last_roi.y < 0)
         last_roi.y = 0;
-    if (last_roi.width < 0)
-        last_roi.width = 0;
-    if (last_roi.height < 0)
-        last_roi.height = 0;
+    if (last_roi.width < 1)
+        last_roi.width = 1;
+    if (last_roi.height < 1)
+        last_roi.height = 1;
     if (last_roi.x >= color.cols-1)
         last_roi.x = color.cols-1;
     if (last_roi.width >= color.cols-1)
@@ -231,6 +235,9 @@ void Tracker::clamp_last_roi()
         last_roi.y = color.rows-1;
     if (last_roi.height >= color.rows-1)
         last_roi.height = color.rows-1;
+
+    last_roi.width -= last_roi.x;
+    last_roi.height -= last_roi.y;
 }
 
 void Tracker::set_points()
@@ -251,30 +258,30 @@ void Tracker::set_points()
 void Tracker::draw_centroid()
 {
     repr2.clear();
+
+    static const std::vector<cv::Point3f> centroid { cv::Point3f(0, 0, 0) };
+
     cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
 
     auto s = cv::Scalar(255, 0, 255);
-    cv::circle(frame, repr2.at(0), 4, s, -1);
+    cv::circle(frame, repr2[0], 4, s, -1);
 }
 
-bool Tracker::set_last_roi()
+void Tracker::set_last_roi()
 {
     roi_projection.clear();
 
-    if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE))
-        return false;
-
+    using f = float;
+    cv::Point3f h(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z));
     for (unsigned i = 0; i < 4; i++)
     {
-        using f = float;
-        cv::Point3f pt(obj_points[i] - cv::Point3f(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z)));
+        cv::Point3f pt(obj_points[i] - h);
         roi_points[i] = pt * c_search_window;
     }
-    cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection);
 
-    set_roi_from_projection();
+    cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
 
-    return true;
+    set_roi_from_projection();
 }
 
 void Tracker::set_rmat()
@@ -316,9 +323,6 @@ void Tracker::set_roi_from_projection()
         last_roi.height = max_y;
     }
 
-    last_roi.width -= last_roi.x;
-    last_roi.height -= last_roi.y;
-
     clamp_last_roi();
 }
 
@@ -346,9 +350,11 @@ void Tracker::run()
         }
 
         cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY);
+
         color.copyTo(frame);
 
         set_intrinsics();
+
         update_fps(alpha_);
 
         markers.clear();
@@ -362,9 +368,7 @@ void Tracker::run()
             if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE))
                 goto fail;
 
-            if (!set_last_roi())
-                goto fail;
-
+            set_last_roi();
             draw_centroid();
             set_rmat();
         }