From e2eeb57be97bb01f51197d0526e46f02658a726c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Tue, 8 Nov 2022 23:44:26 +0100 Subject: [PATCH 1/3] Fix camera focal length --- webots_ros2_driver/src/plugins/static/Ros2Camera.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index 2719ec402..7075453df 100644 --- 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}; From 633f9279fd4f5cf34690e74523ee085387f9a0fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Wed, 9 Nov 2022 11:56:56 +0100 Subject: [PATCH 2/3] Update CHANGELOG.rst --- webots_ros2_driver/CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 6e9575eb4..04b97ba12 100644 --- 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. From 9d3e57ba73c25ed5832bc41bd0d279002222f723 Mon Sep 17 00:00:00 2001 From: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Date: Wed, 9 Nov 2022 12:58:05 +0100 Subject: [PATCH 3/3] Update CHANGELOG.rst --- webots_ros2/CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 52450e55b..b32046d55 100644 --- 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.