correct focal length equation

1 files changed, 3 insertions, 3 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index cbd9f531..77f794b9 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -217,8 +217,8 @@ void Tracker::run()
         const int scale = frame.cols > 480 ? 2 : 1;
         detector.setThresholdParams(scale > 1 ? 11 : 7, 4);
 
-        const float focal_length_w = 0.5 * grayscale.cols / tan(s.fov * HT_PI / 180);
-        const float focal_length_h = 0.5 * grayscale.rows / tan(s.fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
+        const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * s.fov * HT_PI / 180);
+        const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * s.fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
         cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
         intrinsics.at<float> (0, 0) = focal_length_w;
         intrinsics.at<float> (1, 1) = focal_length_h;
@@ -286,7 +286,7 @@ void Tracker::run()
                              cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,
                                               20,
                                               1e-2));
-            const float size = 7;
+            const float size = 3.5;
             
             const double p = s.marker_pitch;
             const double sq = sin(p * HT_PI / 180);