diff --git a/include/gz/rendering/Utils.hh b/include/gz/rendering/Utils.hh index cc552d4d1..82db726fe 100644 --- a/include/gz/rendering/Utils.hh +++ b/include/gz/rendering/Utils.hh @@ -97,6 +97,20 @@ namespace ignition ignition::math::AxisAlignedBox transformAxisAlignedBox( const ignition::math::AxisAlignedBox &_box, const ignition::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. + IGNITION_RENDERING_VISIBLE + ignition::math::Matrix3d projectionToCameraIntrinsic( + const ignition::math::Matrix4d &_projectionMatrix, + double _width, double _height); } } } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index ae2190f67..262f41a86 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -26,6 +26,7 @@ #include "gz/rendering/RenderingIface.hh" #include "gz/rendering/RenderPassSystem.hh" #include "gz/rendering/Scene.hh" +#include "gz/rendering/Utils.hh" using namespace gz; using namespace rendering; @@ -47,6 +48,9 @@ class CameraTest : public testing::Test, /// \brief Test setting visibility mask public: void VisibilityMask(const std::string &_renderEngine); + + /// \brief Test setting intrinsic matrix + public: void IntrinsicMatrix(const std::string &_renderEngine); }; ///////////////////////////////////////////////// @@ -365,6 +369,82 @@ void CameraTest::VisibilityMask(const std::string &_renderEngine) rendering::unloadEngine(engine->Name()); } +///////////////////////////////////////////////// +void CameraTest::IntrinsicMatrix(const std::string &_renderEngine) +{ + // create and populate scene + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + 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); +} + ///////////////////////////////////////////////// TEST_P(CameraTest, ViewProjectionMatrix) { @@ -395,6 +475,12 @@ TEST_P(CameraTest, VisibilityMask) VisibilityMask(GetParam()); } +///////////////////////////////////////////////// +TEST_P(CameraTest, IntrinsicMatrix) +{ + IntrinsicMatrix(GetParam()); +} + INSTANTIATE_TEST_CASE_P(Camera, CameraTest, RENDER_ENGINE_VALUES, PrintToStringParam()); diff --git a/src/Utils.cc b/src/Utils.cc index 39154dd29..eabeeb64d 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -163,6 +163,25 @@ float screenScalingFactor() return ratio; } +///////////////////////////////////////////////// +ignition::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); +} + ///////////////////////////////////////////////// ignition::math::AxisAlignedBox transformAxisAlignedBox( const ignition::math::AxisAlignedBox &_bbox,