Fix camera focal length (#510)

* Fix camera focal length * Update CHANGELOG.rst * Update CHANGELOG.rst Co-authored-by: ad-daniel <[email protected]>
cyberbotics · Nov 9, 2022 · 75118d7 · 75118d7
1 parent 3ffa1de
commit 75118d7
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 3 deletions.
diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package webots_ros2
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.4 (2022-XX-XX)
+------------------
+* Fix the camera focal length in the CameraInfo topic.
+
 2022.1.3 (2022-11-02)
 ------------------
 * Added macOS support.

diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package webots_ros2_driver
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2022.1.4 (2022-XX-XX)
+------------------
+* Fix the camera focal length in the CameraInfo topic.
+
 2022.1.3 (2022-11-02)
 ------------------
 * Added macOS support.

diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp
@@ -44,9 +44,7 @@ namespace webots_ros2_driver
     mCameraInfoMessage.distortion_model = "plumb_bob";
 
     // Convert FoV to focal length.
-    // Reference: https://en.wikipedia.org/wiki/Focal_length#In_photography
-    const double diagonal = sqrt(pow(mCamera->getWidth(), 2) + pow(mCamera->getHeight(), 2));
-    const double focalLength =  0.5 * diagonal * (cos(0.5 * mCamera->getFov()) / sin(0.5 * mCamera->getFov()));
+    const double focalLength = mCamera->getWidth() / (2 * tan(mCamera->getFov() / 2));
 
     mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0};
     mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};