diff --git a/include/gz/rendering/Utils.hh b/include/gz/rendering/Utils.hh index d36b99729..61b16fb4e 100644 --- a/include/gz/rendering/Utils.hh +++ b/include/gz/rendering/Utils.hh @@ -97,6 +97,20 @@ namespace gz gz::math::AxisAlignedBox transformAxisAlignedBox( const gz::math::AxisAlignedBox &_box, const gz::math::Pose3d &_pose); + + /// \brief Convert a given camera projection matrix + /// to an intrinsics matrix. Intrinsics matrix is different + /// from the matrix returned by Camera::ProjectionMatrix(), + /// which is used by OpenGL internally. + /// The matrix returned contains the camera calibrated values. + /// \param[in] _projectionMatrix Camera's projection matrix. + /// \param[in] _width Camera's image width. + /// \param[in] _height Camera's image height. + /// \return Camera's intrinsic matrix. + GZ_RENDERING_VISIBLE + gz::math::Matrix3d projectionToCameraIntrinsic( + const gz::math::Matrix4d &_projectionMatrix, + double _width, double _height); } } } diff --git a/src/Utils.cc b/src/Utils.cc index 3e91ef3c9..a65d6b44f 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -163,6 +163,25 @@ float screenScalingFactor() return ratio; } +///////////////////////////////////////////////// +gz::math::Matrix3d projectionToCameraIntrinsic( + const gz::math::Matrix4d &_projectionMatrix, + double _width, double _height) +{ + // Extracting the intrinsic matrix : + // https://ogrecave.github.io/ogre/api/13/class_ogre_1_1_math.html + double fX = (_projectionMatrix(0, 0) * _width) / 2.0; + double fY = (_projectionMatrix(1, 1) * _height) / 2.0; + double cX = (-1.0 * _width * + (_projectionMatrix(0, 2) - 1.0)) / 2.0; + double cY = _height + (_height * + (_projectionMatrix(1, 2) - 1)) / 2.0; + + return gz::math::Matrix3d(fX, 0, cX, + 0, fY, cY, + 0, 0, 1); +} + ///////////////////////////////////////////////// gz::math::AxisAlignedBox transformAxisAlignedBox( const gz::math::AxisAlignedBox &_bbox, diff --git a/test/common_test/Camera_TEST.cc b/test/common_test/Camera_TEST.cc index 0bfc9d48c..0a522cad6 100644 --- a/test/common_test/Camera_TEST.cc +++ b/test/common_test/Camera_TEST.cc @@ -23,6 +23,7 @@ #include "gz/rendering/GaussianNoisePass.hh" #include "gz/rendering/RenderPassSystem.hh" #include "gz/rendering/Scene.hh" +#include "gz/rendering/Utils.hh" using namespace gz; using namespace rendering; @@ -298,3 +299,70 @@ TEST_F(CameraTest, VisibilityMask) // Clean up engine->DestroyScene(scene); } + +///////////////////////////////////////////////// +TEST_F(CameraTest, IntrinsicMatrix) +{ + ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + + CameraPtr camera = scene->CreateCamera(); + EXPECT_TRUE(camera != nullptr); + + unsigned int width = 320; + unsigned int height = 240; + double hfov = 1.047; + + camera->SetImageHeight(height); + camera->SetImageWidth(width); + camera->SetHFOV(hfov); + + double error = 1e-1; + EXPECT_EQ(camera->ImageHeight(), height); + EXPECT_EQ(camera->ImageWidth(), width); + EXPECT_NEAR(camera->HFOV().Radian(), hfov, error); + + // Verify focal length and optical center from intrinsics + auto cameraIntrinsics = projectionToCameraIntrinsic( + camera->ProjectionMatrix(), + camera->ImageWidth(), + camera->ImageHeight() + ); + EXPECT_NEAR(cameraIntrinsics(0, 0), 277.1913, error); + EXPECT_NEAR(cameraIntrinsics(1, 1), 277.1913, error); + EXPECT_DOUBLE_EQ(cameraIntrinsics(0, 2), 160); + EXPECT_DOUBLE_EQ(cameraIntrinsics(1, 2), 120); + + // Verify rest of the intrinsics + EXPECT_EQ(cameraIntrinsics(0, 1), 0); + EXPECT_EQ(cameraIntrinsics(1, 0), 0); + EXPECT_EQ(cameraIntrinsics(0, 1), 0); + EXPECT_EQ(cameraIntrinsics(2, 0), 0); + EXPECT_EQ(cameraIntrinsics(2, 1), 0); + EXPECT_EQ(cameraIntrinsics(2, 2), 1); + + // Verify that changing camera size changes intrinsics + height = 1000; + width = 1000; + camera->SetImageHeight(height); + camera->SetImageWidth(width); + camera->SetHFOV(hfov); + + EXPECT_EQ(camera->ImageHeight(), height); + EXPECT_EQ(camera->ImageWidth(), width); + EXPECT_NEAR(camera->HFOV().Radian(), hfov, error); + + // Verify if intrinsics have changed + cameraIntrinsics = projectionToCameraIntrinsic( + camera->ProjectionMatrix(), + camera->ImageWidth(), + camera->ImageHeight() + ); + EXPECT_NEAR(cameraIntrinsics(0, 0), 866.223, error); + EXPECT_NEAR(cameraIntrinsics(1, 1), 866.223, error); + EXPECT_DOUBLE_EQ(cameraIntrinsics(0, 2), 500); + EXPECT_DOUBLE_EQ(cameraIntrinsics(1, 2), 500); + + // Clean up + engine->DestroyScene(scene); +}