From bd695b2554e79e2f78e1af774ca4d474fd3cca3f Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Wed, 4 Oct 2023 09:53:57 -0700 Subject: [PATCH] Set lens intrinsics in Depth and Rgbd camera sensors (#390) * Refactor projection matrix utility functions to CameraSensorUtil and set lens intrinsics in Depth and Rgbd camera sensors --------- Signed-off-by: Shameek Ganguly --- src/CMakeLists.txt | 1 + src/CameraSensor.cc | 139 +-------------------------------------- src/CameraSensorUtil.cc | 91 +++++++++++++++++++++++++ src/CameraSensorUtil.hh | 94 ++++++++++++++++++++++++++ src/DepthCameraSensor.cc | 79 +++++++++++++++++++++- src/RgbdCameraSensor.cc | 79 +++++++++++++++++++++- 6 files changed, 341 insertions(+), 142 deletions(-) create mode 100644 src/CameraSensorUtil.cc create mode 100644 src/CameraSensorUtil.hh diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 92a56178..23e52bb0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,6 @@ set (sources BrownDistortionModel.cc + CameraSensorUtil.cc Distortion.cc EnvironmentalData.cc GaussianNoiseModel.cc diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index e8f99e76..1d4cf483 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -31,6 +31,7 @@ #include #include +#include "CameraSensorUtil.hh" #include "gz/sensors/CameraSensor.hh" #include "gz/sensors/ImageBrownDistortionModel.hh" #include "gz/sensors/ImageDistortion.hh" @@ -61,69 +62,6 @@ class gz::sensors::CameraSensorPrivate public: bool SaveImage(const unsigned char *_data, unsigned int _width, unsigned int _height, gz::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; @@ -320,7 +258,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, @@ -374,7 +312,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, @@ -919,74 +857,3 @@ bool CameraSensor::HasInfoConnections() const { return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections(); } - -////////////////////////////////////////////////// -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..ed76c986 --- /dev/null +++ b/src/CameraSensorUtil.hh @@ -0,0 +1,94 @@ +/* + * 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" + +namespace gz +{ +namespace sensors +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_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 +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 +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 +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 e295d0e5..48648894 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -29,6 +29,7 @@ #include #include +#include #include #include "gz/sensors/DepthCameraSensor.hh" @@ -38,6 +39,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 @@ -325,7 +327,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) { @@ -339,8 +341,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); @@ -393,6 +393,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 9009b658..d7ff1c27 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -36,6 +37,7 @@ #include "gz/sensors/RenderingEvents.hh" #include "gz/sensors/SensorFactory.hh" +#include "CameraSensorUtil.hh" #include "PointCloudUtil.hh" /// \brief Private data for RgbdCameraSensor @@ -266,7 +268,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) { @@ -274,8 +276,6 @@ bool RgbdCameraSensor::CreateCameras() return false; } - this->PopulateInfo(cameraSdf); - int width = cameraSdf->ImageWidth(); int height = cameraSdf->ImageHeight(); @@ -359,6 +359,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();