1 files changed, 62 insertions, 11 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 43ffc717..b532ac9c 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -185,18 +185,28 @@ void Tracker::run()
         fprintf(stderr, "aruco tracker: can't open camera\n");
         return;
     }
-  
+
+    auto freq = cv::getTickFrequency();
+    auto last_time = cv::getTickCount();
+    auto prev_time = last_time;
+    int fps = 0;
+    int last_fps = 0;
+    double error = 0;
+    std::vector<cv::Point2f> reprojection;
+    auto kernel = cv::createGaussianFilter(CV_8U, cv::Size(5, 5), 0);
     while (!stop)
     {
         if (!camera.read(color_))
             continue;
         color_.copyTo(color);
         cv::cvtColor(color, grayscale2, cv::COLOR_BGR2GRAY);
-        const int kernel = grayscale2.cols > 480 ? 7 : 5;
-        int kernel2 = kernel * grayscale2.rows / grayscale2.cols - 1;
-        if ((kernel2 % 2) == 0)
-            kernel2++;
-        cv::GaussianBlur(grayscale2, grayscale, cv::Size(kernel, kernel2), 0, 0);
+        if (grayscale.empty())
+            grayscale = grayscale2.clone();
+
+        kernel->apply(grayscale2, grayscale);
+
+        const int scale = frame.cols > 480 ? 2 : 1;
+
         const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * fov * HT_PI / 180);
         const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
         cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
@@ -204,7 +214,7 @@ void Tracker::run()
         intrinsics.at<float> (1, 1) = focal_length_h;
         intrinsics.at<float> (0, 2) = grayscale.cols/2;
         intrinsics.at<float> (1, 2) = grayscale.rows/2;
-        
+
         cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
         
         for (int i = 0; i < 5; i++)
@@ -217,11 +227,40 @@ void Tracker::run()
         if (markers.size() == 1 && markers[0].size() == 4) {
             const aruco::Marker& m = markers.at(0);
             for (int i = 0; i < 4; i++)
-                cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 4);
+                cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale);
         }
-        
+
+        for (int i = 0; i < reprojection.size(); i++)
+        {
+            cv::circle(frame,
+                       reprojection[i],
+                       6,
+                       cv::Scalar(0, 255, 128),
+                       3);
+        }
+
+        auto time = cv::getTickCount();
+
+        if ((long) (time / freq) != (long) (last_time / freq))
+        {
+            last_fps = fps;
+            fps = 0;
+            last_time = time;
+        }
+
+        fps++;
+
+        char buf[128];
+
+        std::sprintf(buf, "Hz: %ld", last_fps);
+        cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
+        std::sprintf(buf, "Delay: %ld ms", (long) (1000 * (time - prev_time) / freq));
+        cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
+        std::sprintf(buf, "Error: %f px", error);
+        cv::putText(frame, buf, cv::Point(10, 76), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
+        prev_time = time;
+
         frame = color;
-        
         if (frame.rows > 0)
         {
             videoWidget->update_image(frame.data, frame.cols, frame.rows);
@@ -263,7 +302,19 @@ void Tracker::run()
             pose[Yaw] = foo[1];
             pose[Pitch] = -foo[0];
             pose[Roll] = foo[2];
-            
+
+            reprojection.clear();
+            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;
         }