aruco: show reprojection error

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

1 files changed, 13 insertions, 2 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 2bb3ad77..5de62176 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -191,6 +191,7 @@ void Tracker::run()
     auto prev_time = last_time;
     int fps = 0;
     int last_fps = 0;
+    double error = 0;
     std::vector<cv::Point2f> reprojection;
   
     while (!stop)
@@ -250,9 +251,11 @@ void Tracker::run()
         char buf[128];
 
         std::sprintf(buf, "Hz: %ld", last_fps);
-        cv::putText(frame, buf, cv::Point(10, 30), cv::FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(0, 255, 0), 2);
+        cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(0, 255, 0), 2);
         std::sprintf(buf, "Delay: %ld ms", (long) (1000 * (time - prev_time) / freq));
-        cv::putText(frame, buf, cv::Point(10, 50), cv::FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(0, 255, 0), 2);
+        cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(80, 255, 0), 2);
+        std::sprintf(buf, "Error: %f px", error);
+        cv::putText(frame, buf, cv::Point(10, 76), cv::FONT_HERSHEY_PLAIN, 2.0, cv::Scalar(80, 255, 0), 2);
         prev_time = time;
 
         frame = color;
@@ -302,6 +305,14 @@ void Tracker::run()
             reprojection.resize(4);
             cv::projectPoints(obj_points, rvec, tvec, intrinsics, dist_coeffs, reprojection);
 
+            error = 0;
+            for (int i = 0; i < 4; i++)
+            {
+                double x = reprojection[i].x - m[i].x;
+                double y = reprojection[i].y - m[i].y;
+                error += std::sqrt(x * x + y * y);
+            }
+
             //pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI;
             //pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI;
         }