pt, ht, aruco: use calibration data. rename fov to diagonal in UI

1 files changed, 6 insertions, 3 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index dc0659e5..5b9ec8b6 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -54,9 +54,12 @@ void Tracker_PT::reset_command(Command command)
 
 float Tracker_PT::get_focal_length()
 {
-    static constexpr float pi = 3.1415926f;
-    const float fov = static_cast<int>(s.fov) * pi / 180.f;
-    return 0.5f / tan(0.5f * fov);
+    CamInfo info = camera.get_info();
+    const int w = info.res_x, h = info.res_y;
+    static constexpr double pi = 3.1415926f;
+    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.0)/sqrt(1. + diag*diag));
+    return .5 / tan(.5 * fov);
 }
 
 void Tracker_PT::run()