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

2 files changed, 36 insertions, 33 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();
         }
diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h
index fb207f6f..94a96e4c 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.h
+++ b/tracker-aruco/ftnoir_tracker_aruco.h
@@ -70,7 +70,7 @@ private:
     void clamp_last_roi();
     void set_points();
     void draw_centroid();
-    bool set_last_roi();
+    void set_last_roi();
     void set_rmat();
     void set_roi_from_projection();
 
@@ -90,10 +90,9 @@ private:
     aruco::MarkerDetector detector;
     std::vector<aruco::Marker> markers;
     cv::Vec3d t;
-    cv::Vec3d rvec, tvec, rvec_, tvec_;
+    cv::Vec3d rvec, tvec;
     std::vector<cv::Point2f> roi_projection;
     std::vector<cv::Point2f> repr2;
-    std::vector<cv::Point3f> centroid;
     cv::Matx33d m_r, m_q, rmat;
     cv::Vec3d euler;
     std::vector<cv::Point3f> roi_points;
@@ -104,7 +103,7 @@ private:
     static constexpr float size_min = 0.05f;
     static constexpr float size_max = 0.3f;
 
-    static constexpr double alpha_ = .985;
+    static constexpr double alpha_ = .95;
 };
 
 class TrackerControls : public ITrackerDialog, protected camera_dialog