From 7cd222e556c2cf1a635475c2b816bde105981a5d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 4 Dec 2024 15:30:25 +0100 Subject: [PATCH] Set lens intrinsics in Depth and Rgbd camera sensors (backport #390) (#486) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Co-authored-by: shameekganguly Co-authored-by: Alejandro Hernández Cordero --- src/CMakeLists.txt | 3 +- src/CameraSensor.cc | 139 +-------------------------------------- src/CameraSensorUtil.cc | 91 +++++++++++++++++++++++++ src/CameraSensorUtil.hh | 108 ++++++++++++++++++++++++++++++ src/DepthCameraSensor.cc | 80 +++++++++++++++++++++- src/RgbdCameraSensor.cc | 84 +++++++++++++++++++++-- 6 files changed, 360 insertions(+), 145 deletions(-) create mode 100644 src/CameraSensorUtil.cc create mode 100644 src/CameraSensorUtil.hh diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index cd072ae3..8476b7d5 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,6 @@ set (sources BrownDistortionModel.cc + CameraSensorUtil.cc Distortion.cc GaussianNoiseModel.cc Manager.cc @@ -49,7 +50,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} ignition-common${IGN_COMMON_VER}::profiler ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} ) -target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC DepthPoints_EXPORTS) +target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC DepthPoints_EXPORTS CameraSensorUtil_EXPORTS) ign_add_component(rendering SOURCES ${rendering_sources} GET_TARGET_NAME rendering_target) target_link_libraries(${rendering_target} diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index dc06d64f..6ab719ce 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -35,6 +35,7 @@ #include #include +#include "CameraSensorUtil.hh" #include "gz/sensors/CameraSensor.hh" #include "gz/sensors/ImageBrownDistortionModel.hh" #include "gz/sensors/ImageDistortion.hh" @@ -69,69 +70,6 @@ class gz::sensors::CameraSensorPrivate public: bool SaveImage(const unsigned char *_data, unsigned int _width, unsigned int _height, common::Image::PixelFormatType _format); - /// \brief Computes the OpenGL NDC matrix - /// \param[in] _left Left vertical clipping plane - /// \param[in] _right Right vertical clipping plane - /// \param[in] _bottom Bottom horizontal clipping plane - /// \param[in] _top Top horizontal clipping plane - /// \param[in] _near Distance to the nearer depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \param[in] _far Distance to the farther depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \return OpenGL NDC (Normalized Device Coordinates) matrix - public: static math::Matrix4d BuildNDCMatrix( - double _left, double _right, - double _bottom, double _top, - double _near, double _far); - - /// \brief Computes the OpenGL perspective matrix - /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) - /// \param[in] _intrinsicsFy Vertical focal length (in pixels) - /// \param[in] _intrinsicsCx X coordinate of principal point in pixels - /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels - /// \param[in] _intrinsicsS Skew coefficient defining the angle between - /// the x and y pixel axes - /// \param[in] _clipNear Distance to the nearer depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \param[in] _clipFar Distance to the farther depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \return OpenGL perspective matrix - public: static math::Matrix4d BuildPerspectiveMatrix( - double _intrinsicsFx, double _intrinsicsFy, - double _intrinsicsCx, double _intrinsicsCy, - double _intrinsicsS, - double _clipNear, double _clipFar); - - /// \brief Computes the OpenGL projection matrix by multiplying - /// the OpenGL Normalized Device Coordinates matrix (NDC) with - /// the OpenGL perspective matrix - /// openglProjectionMatrix = ndcMatrix * perspectiveMatrix - /// \param[in] _imageWidth Image width (in pixels) - /// \param[in] _imageHeight Image height (in pixels) - /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) - /// \param[in] _intrinsicsFy Vertical focal length (in pixels) - /// \param[in] _intrinsicsCx X coordinate of principal point in pixels - /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels - /// \param[in] _intrinsicsS Skew coefficient defining the angle between - /// the x and y pixel axes - /// \param[in] _clipNear Distance to the nearer depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \param[in] _clipFar Distance to the farther depth clipping plane - /// This value is negative if the plane is to be behind - /// the camera - /// \return OpenGL projection matrix - public: static math::Matrix4d BuildProjectionMatrix( - double _imageWidth, double _imageHeight, - double _intrinsicsFx, double _intrinsicsFy, - double _intrinsicsCx, double _intrinsicsCy, - double _intrinsicsS, - double _clipNear, double _clipFar); - /// \brief node to create publisher public: transport::Node node; @@ -323,7 +261,7 @@ bool CameraSensor::CreateCamera() double cx = cameraSdf->LensIntrinsicsCx(); double cy = cameraSdf->LensIntrinsicsCy(); double s = cameraSdf->LensIntrinsicsSkew(); - auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + auto projectionMatrix = buildProjectionMatrix( this->dataPtr->camera->ImageWidth(), this->dataPtr->camera->ImageHeight(), fx, fy, cx, cy, s, @@ -377,7 +315,7 @@ bool CameraSensor::CreateCamera() double cy = cameraSdf->LensProjectionCy(); double s = 0; - auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + auto projectionMatrix = buildProjectionMatrix( this->dataPtr->camera->ImageWidth(), this->dataPtr->camera->ImageHeight(), fx, fy, cx, cy, s, @@ -907,74 +845,3 @@ const std::string &CameraSensor::OpticalFrameId() const { return this->dataPtr->opticalFrameId; } - -////////////////////////////////////////////////// -math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix( - double _imageWidth, double _imageHeight, - double _intrinsicsFx, double _intrinsicsFy, - double _intrinsicsCx, double _intrinsicsCy, - double _intrinsicsS, - double _clipNear, double _clipFar) -{ - return CameraSensorPrivate::BuildNDCMatrix( - 0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) * - CameraSensorPrivate::BuildPerspectiveMatrix( - _intrinsicsFx, _intrinsicsFy, - _intrinsicsCx, _imageHeight - _intrinsicsCy, - _intrinsicsS, _clipNear, _clipFar); -} - -////////////////////////////////////////////////// -math::Matrix4d CameraSensorPrivate::BuildNDCMatrix( - double _left, double _right, - double _bottom, double _top, - double _near, double _far) -{ - double inverseWidth = 1.0 / (_right - _left); - double inverseHeight = 1.0 / (_top - _bottom); - double inverseDistance = 1.0 / (_far - _near); - - return math::Matrix4d( - 2.0 * inverseWidth, - 0.0, - 0.0, - -(_right + _left) * inverseWidth, - 0.0, - 2.0 * inverseHeight, - 0.0, - -(_top + _bottom) * inverseHeight, - 0.0, - 0.0, - -2.0 * inverseDistance, - -(_far + _near) * inverseDistance, - 0.0, - 0.0, - 0.0, - 1.0); -} - -////////////////////////////////////////////////// -math::Matrix4d CameraSensorPrivate::BuildPerspectiveMatrix( - double _intrinsicsFx, double _intrinsicsFy, - double _intrinsicsCx, double _intrinsicsCy, - double _intrinsicsS, - double _clipNear, double _clipFar) -{ - return math::Matrix4d( - _intrinsicsFx, - _intrinsicsS, - -_intrinsicsCx, - 0.0, - 0.0, - _intrinsicsFy, - -_intrinsicsCy, - 0.0, - 0.0, - 0.0, - _clipNear + _clipFar, - _clipNear * _clipFar, - 0.0, - 0.0, - -1.0, - 0.0); -} diff --git a/src/CameraSensorUtil.cc b/src/CameraSensorUtil.cc new file mode 100644 index 00000000..dc080c7c --- /dev/null +++ b/src/CameraSensorUtil.cc @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "CameraSensorUtil.hh" + +#include + +////////////////////////////////////////////////// +gz::math::Matrix4d gz::sensors::buildNDCMatrix( + double _left, double _right, + double _bottom, double _top, + double _near, double _far) +{ + double inverseWidth = 1.0 / (_right - _left); + double inverseHeight = 1.0 / (_top - _bottom); + double inverseDistance = 1.0 / (_far - _near); + + return math::Matrix4d( + 2.0 * inverseWidth, + 0.0, + 0.0, + -(_right + _left) * inverseWidth, + 0.0, + 2.0 * inverseHeight, + 0.0, + -(_top + _bottom) * inverseHeight, + 0.0, + 0.0, + -2.0 * inverseDistance, + -(_far + _near) * inverseDistance, + 0.0, + 0.0, + 0.0, + 1.0); +} + +////////////////////////////////////////////////// +gz::math::Matrix4d gz::sensors::buildPerspectiveMatrix( + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar) +{ + return math::Matrix4d( + _intrinsicsFx, + _intrinsicsS, + -_intrinsicsCx, + 0.0, + 0.0, + _intrinsicsFy, + -_intrinsicsCy, + 0.0, + 0.0, + 0.0, + _clipNear + _clipFar, + _clipNear * _clipFar, + 0.0, + 0.0, + -1.0, + 0.0); +} + +////////////////////////////////////////////////// +gz::math::Matrix4d gz::sensors::buildProjectionMatrix( + double _imageWidth, double _imageHeight, + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar) +{ + return buildNDCMatrix( + 0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) * + buildPerspectiveMatrix( + _intrinsicsFx, _intrinsicsFy, + _intrinsicsCx, _imageHeight - _intrinsicsCy, + _intrinsicsS, _clipNear, _clipFar); +} diff --git a/src/CameraSensorUtil.hh b/src/CameraSensorUtil.hh new file mode 100644 index 00000000..0fb363e5 --- /dev/null +++ b/src/CameraSensorUtil.hh @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_SENSORS_CAMERASENSORUTIL_HH_ +#define GZ_SENSORS_CAMERASENSORUTIL_HH_ + +#include + +#include "gz/sensors/config.hh" +#include "gz/sensors/Export.hh" + +#ifndef _WIN32 +# define CameraSensorUtil_EXPORTS_API +#else +# if (defined(CameraSensorUtil_EXPORTS)) +# define CameraSensorUtil_EXPORTS_API __declspec(dllexport) +# else +# define CameraSensorUtil_EXPORTS_API __declspec(dllimport) +# endif +#endif + + +namespace ignition +{ +namespace sensors +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { +/// \brief Computes the OpenGL NDC matrix +/// \param[in] _left Left vertical clipping plane +/// \param[in] _right Right vertical clipping plane +/// \param[in] _bottom Bottom horizontal clipping plane +/// \param[in] _top Top horizontal clipping plane +/// \param[in] _near Distance to the nearer depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \param[in] _far Distance to the farther depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \return OpenGL NDC (Normalized Device Coordinates) matrix +CameraSensorUtil_EXPORTS_API +math::Matrix4d buildNDCMatrix(double _left, double _right, double _bottom, + double _top, double _near, double _far); + +/// \brief Computes the OpenGL perspective matrix +/// \param[in] _intrinsicsFx Horizontal focal length (in pixels) +/// \param[in] _intrinsicsFy Vertical focal length (in pixels) +/// \param[in] _intrinsicsCx X coordinate of principal point in pixels +/// \param[in] _intrinsicsCy Y coordinate of principal point in pixels +/// \param[in] _intrinsicsS Skew coefficient defining the angle between +/// the x and y pixel axes +/// \param[in] _clipNear Distance to the nearer depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \param[in] _clipFar Distance to the farther depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \return OpenGL perspective matrix +CameraSensorUtil_EXPORTS_API +math::Matrix4d buildPerspectiveMatrix( + double _intrinsicsFx, double _intrinsicsFy, double _intrinsicsCx, + double _intrinsicsCy, double _intrinsicsS, double _clipNear, + double _clipFar); + +/// \brief Computes the OpenGL projection matrix by multiplying +/// the OpenGL Normalized Device Coordinates matrix (NDC) with +/// the OpenGL perspective matrix +/// openglProjectionMatrix = ndcMatrix * perspectiveMatrix +/// \param[in] _imageWidth Image width (in pixels) +/// \param[in] _imageHeight Image height (in pixels) +/// \param[in] _intrinsicsFx Horizontal focal length (in pixels) +/// \param[in] _intrinsicsFy Vertical focal length (in pixels) +/// \param[in] _intrinsicsCx X coordinate of principal point in pixels +/// \param[in] _intrinsicsCy Y coordinate of principal point in pixels +/// \param[in] _intrinsicsS Skew coefficient defining the angle between +/// the x and y pixel axes +/// \param[in] _clipNear Distance to the nearer depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \param[in] _clipFar Distance to the farther depth clipping plane +/// This value is negative if the plane is to be behind +/// the camera +/// \return OpenGL projection matrix +CameraSensorUtil_EXPORTS_API +math::Matrix4d buildProjectionMatrix(double _imageWidth, double _imageHeight, + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, double _clipNear, + double _clipFar); +} +} +} + +#endif diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 14c48c3a..6e051d66 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -34,6 +34,8 @@ #include #include +#include +#include #include #include "gz/sensors/DepthCameraSensor.hh" @@ -43,6 +45,7 @@ #include "gz/sensors/ImageNoise.hh" #include "gz/sensors/RenderingEvents.hh" +#include "CameraSensorUtil.hh" #include "PointCloudUtil.hh" // undefine near and far macros from windows.h @@ -367,7 +370,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) ////////////////////////////////////////////////// bool DepthCameraSensor::CreateCamera() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { @@ -388,8 +391,6 @@ bool DepthCameraSensor::CreateCamera() double far = cameraSdf->FarClip(); double near = cameraSdf->NearClip(); - this->PopulateInfo(cameraSdf); - this->dataPtr->depthCamera = this->Scene()->CreateDepthCamera( this->Name()); this->dataPtr->depthCamera->SetImageWidth(width); @@ -442,6 +443,79 @@ bool DepthCameraSensor::CreateCamera() this->dataPtr->depthCamera->SetAspectRatio(static_cast(width)/height); this->dataPtr->depthCamera->SetHFOV(angle); + // Update the DOM object intrinsics to have consistent + // intrinsics between ogre camera and camera_info msg + if (!cameraSdf->HasLensIntrinsics()) { + auto intrinsicMatrix = gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->depthCamera->ProjectionMatrix(), + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight()); + + cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); + } + else + { + // set custom projection matrix based on intrinsics param specified in sdf + double fx = cameraSdf->LensIntrinsicsFx(); + double fy = cameraSdf->LensIntrinsicsFy(); + double cx = cameraSdf->LensIntrinsicsCx(); + double cy = cameraSdf->LensIntrinsicsCy(); + double s = cameraSdf->LensIntrinsicsSkew(); + auto projectionMatrix = buildProjectionMatrix( + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight(), fx, fy, cx, cy, s, + this->dataPtr->depthCamera->NearClipPlane(), + this->dataPtr->depthCamera->FarClipPlane()); + this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix); + } + + // Update the DOM object intrinsics to have consistent + // projection matrix values between ogre camera and camera_info msg + // If these values are not defined in the SDF then we need to update + // these values to something reasonable. The projection matrix is + // the cumulative effect of intrinsic and extrinsic parameters + if(!cameraSdf->HasLensProjection()) + { + // Note that the matrix from Ogre via camera->ProjectionMatrix() has a + // different format than the projection matrix used in SDFormat. + // This is why they are converted using projectionToCameraIntrinsic. + // The resulting matrix is the intrinsic matrix, but since the user has + // not overridden the values, this is also equal to the projection matrix. + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->depthCamera->ProjectionMatrix(), + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight() + ); + cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2)); + } + else + { + // set custom projection matrix based on projection param specified in sdf + // tx and ty are not used + double fx = cameraSdf->LensProjectionFx(); + double fy = cameraSdf->LensProjectionFy(); + double cx = cameraSdf->LensProjectionCx(); + double cy = cameraSdf->LensProjectionCy(); + double s = 0; + + auto projectionMatrix = buildProjectionMatrix( + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight(), + fx, fy, cx, cy, s, + this->dataPtr->depthCamera->NearClipPlane(), + this->dataPtr->depthCamera->FarClipPlane()); + this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix); + } + + this->PopulateInfo(cameraSdf); + // Create depth texture when the camera is reconfigured from default values this->dataPtr->depthCamera->CreateDepthTexture(); diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 6b7758d0..5879a17d 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -33,6 +33,8 @@ #include #include +#include +#include #include #include @@ -43,6 +45,7 @@ #include "gz/sensors/RenderingEvents.hh" #include "gz/sensors/SensorFactory.hh" +#include "CameraSensorUtil.hh" #include "PointCloudUtil.hh" /// \brief Private data for RgbdCameraSensor @@ -264,7 +267,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) ////////////////////////////////////////////////// bool RgbdCameraSensor::CreateCameras() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { @@ -272,10 +275,8 @@ bool RgbdCameraSensor::CreateCameras() return false; } - this->PopulateInfo(cameraSdf); - - unsigned int width = cameraSdf->ImageWidth(); - unsigned int height = cameraSdf->ImageHeight(); + int width = cameraSdf->ImageWidth(); + int height = cameraSdf->ImageHeight(); if (width == 0u || height == 0u) { @@ -364,6 +365,79 @@ bool RgbdCameraSensor::CreateCameras() this->dataPtr->depthCamera->SetAspectRatio(static_cast(width)/height); this->dataPtr->depthCamera->SetHFOV(angle); + // Update the DOM object intrinsics to have consistent + // intrinsics between ogre camera and camera_info msg + if (!cameraSdf->HasLensIntrinsics()) { + auto intrinsicMatrix = gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->depthCamera->ProjectionMatrix(), + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight()); + + cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); + } + else + { + // set custom projection matrix based on intrinsics param specified in sdf + double fx = cameraSdf->LensIntrinsicsFx(); + double fy = cameraSdf->LensIntrinsicsFy(); + double cx = cameraSdf->LensIntrinsicsCx(); + double cy = cameraSdf->LensIntrinsicsCy(); + double s = cameraSdf->LensIntrinsicsSkew(); + auto projectionMatrix = buildProjectionMatrix( + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight(), fx, fy, cx, cy, s, + this->dataPtr->depthCamera->NearClipPlane(), + this->dataPtr->depthCamera->FarClipPlane()); + this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix); + } + + // Update the DOM object intrinsics to have consistent + // projection matrix values between ogre camera and camera_info msg + // If these values are not defined in the SDF then we need to update + // these values to something reasonable. The projection matrix is + // the cumulative effect of intrinsic and extrinsic parameters + if(!cameraSdf->HasLensProjection()) + { + // Note that the matrix from Ogre via camera->ProjectionMatrix() has a + // different format than the projection matrix used in SDFormat. + // This is why they are converted using projectionToCameraIntrinsic. + // The resulting matrix is the intrinsic matrix, but since the user has + // not overridden the values, this is also equal to the projection matrix. + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->depthCamera->ProjectionMatrix(), + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight() + ); + cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2)); + } + else + { + // set custom projection matrix based on projection param specified in sdf + // tx and ty are not used + double fx = cameraSdf->LensProjectionFx(); + double fy = cameraSdf->LensProjectionFy(); + double cx = cameraSdf->LensProjectionCx(); + double cy = cameraSdf->LensProjectionCy(); + double s = 0; + + auto projectionMatrix = buildProjectionMatrix( + this->dataPtr->depthCamera->ImageWidth(), + this->dataPtr->depthCamera->ImageHeight(), + fx, fy, cx, cy, s, + this->dataPtr->depthCamera->NearClipPlane(), + this->dataPtr->depthCamera->FarClipPlane()); + this->dataPtr->depthCamera->SetProjectionMatrix(projectionMatrix); + } + + this->PopulateInfo(cameraSdf); + // Create depth texture when the camera is reconfigured from default values this->dataPtr->depthCamera->CreateDepthTexture();