correct focal length equation

3 files changed, 7 insertions, 7 deletions

diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index 96d9d972..885b327a 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -250,8 +250,8 @@ void PointTracker::POSIT(float fov, int w, int h)
 

     const float HT_PI = 3.1415926535;

 

-    const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180);

-    const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0);

+    const float focal_length_w = 0.5 * w / tan(0.5 * fov * HT_PI / 180);

+    const float focal_length_h = 0.5 * h / tan(0.5 * fov * h / w * HT_PI / 180.0);

 

     cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);

     intrinsics.at<float> (0, 0) = focal_length_w;

diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h
index 741d5af4..69eb9bba 100644
--- a/FTNoIR_Tracker_PT/point_tracker.h
+++ b/FTNoIR_Tracker_PT/point_tracker.h
@@ -104,8 +104,8 @@ protected:
         if (!rvec.empty() && !tvec.empty() && fov > 0)

         {

             const float HT_PI = 3.1415926535;

-            const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180);

-            const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0);

+            const float focal_length_w = 0.5 * w / tan(0.5 * fov * HT_PI / 180);

+            const float focal_length_h = 0.5 * h / tan(0.5 * fov * h / w * HT_PI / 180.0);

 

             cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);

             intrinsics.at<float> (0, 0) = focal_length_w;

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);