pt, aruco: another take at focal length formula

Aspect ratio is meant to be viewport's ratio, not diagonal to axis.

1 files changed, 5 insertions, 4 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 84f20a61..fa783fe9 100755
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -154,10 +154,11 @@ void Tracker::run()
 
         static constexpr double pi = 3.1415926f;
         const int w = grayscale.cols, h = grayscale.rows;
-        const double diag = sqrt(w * w + h * h)/w, diag_fov = static_cast<int>(s.fov) * pi / 180.;
-        const double fov = 2.*atan(tan(diag_fov/2.)/sqrt(1. + diag*diag));
-        const float focal_length_w = .5 * w / tan(.5 * fov);
-        const float focal_length_h = focal_length_w;
+        const double diag_fov = static_cast<int>(s.fov) * pi / 180.;
+        const double fov_w = 2.*atan(tan(diag_fov/2.)/sqrt(1. + h/(double)w * h/(double)w));
+        const double fov_h = 2.*atan(tan(diag_fov/2.)/sqrt(1. + w/(double)h * w/(double)h));
+        const float focal_length_w = .5 * w / tan(.5 * fov_w);
+        const float focal_length_h = .5 * h / tan(.5 * fov_h);
 
         intrinsics.at<float> (0, 0) = focal_length_w;
         intrinsics.at<float> (1, 1) = focal_length_h;