Merge branch 'unstable' into trackhat-ui

2 files changed, 44 insertions, 40 deletions

diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp
index eaf51e17..9168a3e4 100644
--- a/ftnoir_tracker_pt/camera.cpp
+++ b/ftnoir_tracker_pt/camera.cpp
@@ -16,39 +16,39 @@ using namespace cv;
 // ----------------------------------------------------------------------------
 void get_camera_device_names(std::vector<std::string>& device_names)
 {
-	videoInput VI;
-	VI.listDevices();
-	std::string device_name;
-	for(int index = 0; ; ++index) {
-		device_name = VI.getDeviceName(index);
-		if (device_name.empty()) break;
-		device_names.push_back(device_name);
-	}
+    videoInput VI;
+    VI.listDevices();
+    std::string device_name;
+    for(int index = 0; ; ++index) {
+        device_name = VI.getDeviceName(index);
+        if (device_name.empty()) break;
+        device_names.push_back(device_name);
+    }
 }
 #endif
 
 // ----------------------------------------------------------------------------
 void Camera::set_device_index(int index)
 {
-	if (desired_index != index)
-	{
-		desired_index = index;
-		_set_device_index();
+    if (desired_index != index)
+    {
+        desired_index = index;
+        _set_device_index();
 
-		// reset fps
-		dt_valid = 0;
-		dt_mean = 0;
-		active_index = index;
-	}
+        // reset fps
+        dt_valid = 0;
+        dt_mean = 0;
+        active_index = index;
+    }
 }
 
 void Camera::set_fps(int fps)
 {
-	if (cam_desired.fps != fps)
-	{
-		cam_desired.fps = fps;
-		_set_fps();
-	}
+    if (cam_desired.fps != fps)
+    {
+        cam_desired.fps = fps;
+        _set_fps();
+    }
 }
 
 void Camera::set_res(int x_res, int y_res)
@@ -63,17 +63,17 @@ void Camera::set_res(int x_res, int y_res)
 
 bool Camera::get_frame(float dt, cv::Mat* frame)
 {
-	bool new_frame = _get_frame(frame);
-	// measure fps of valid frames
-	const float dt_smoothing_const = 0.9;
-	dt_valid += dt;
-	if (new_frame)
-	{
-		dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid;
-		cam_info.fps = dt_mean > 1e-3 ? 1.0 / dt_mean : 0;
-		dt_valid = 0;
-	}
-	return new_frame;
+    bool new_frame = _get_frame(frame);
+    // measure fps of valid frames
+    const float dt_smoothing_const = 0.9;
+    dt_valid += dt;
+    if (new_frame)
+    {
+        dt_mean = dt_smoothing_const * dt_mean + (1.0 - dt_smoothing_const) * dt_valid;
+        cam_info.fps = dt_mean > 1e-3 ? 1.0 / dt_mean : 0;
+        dt_valid = 0;
+    }
+    return new_frame;
 }
 
 void CVCamera::start()
@@ -118,10 +118,10 @@ bool CVCamera::_get_frame(Mat* frame)
         if (img.empty())
             return false;
 
-            *frame = img;
-            cam_info.res_x = img.cols;
-            cam_info.res_y = img.rows;
-            return true;
+        *frame = img;
+        cam_info.res_x = img.cols;
+        cam_info.res_y = img.rows;
+        return true;
     }
     return false;
 }
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index d48f9252..d3cf18c1 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -54,7 +54,7 @@ void Tracker_PT::reset_command(Command command)
 
 float Tracker_PT::get_focal_length()
 {
-    static constexpr float pi = 3.1415926f;
+    static constexpr float pi = 3.1415926;
     float fov_;
     switch (s.fov)
     {
@@ -67,8 +67,12 @@ float Tracker_PT::get_focal_length()
         break;
     }
 
-    const float fov = static_cast<int>(fov_) * pi / 180.f;
-    return 0.5f / tan(0.5f * fov);
+    const float diag_fov = static_cast<int>(fov_) * pi / 180.f;
+    CamInfo info = camera.get_info();
+    const int w = info.res_x, h = info.res_y;
+    const double diag = sqrt(w * w + h * h)/w;
+    const double fov = 2.*atan(tan(diag_fov/2.0)/sqrt(1. + diag*diag));
+    return .5 / tan(.5 * fov);
 }
 
 void Tracker_PT::run()