From 75118d78d545d65bfb90c00f5e275766472fe999 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Wed, 9 Nov 2022 14:20:30 +0100 Subject: [PATCH] Fix camera focal length (#510) * Fix camera focal length * Update CHANGELOG.rst * Update CHANGELOG.rst Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> --- webots_ros2/CHANGELOG.rst | 4 ++++ webots_ros2_driver/CHANGELOG.rst | 4 ++++ webots_ros2_driver/src/plugins/static/Ros2Camera.cpp | 4 +--- 3 files changed, 9 insertions(+), 3 deletions(-) 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. 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. 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};