[backport fortress] Calculate camera intrinsics : Refactor

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);
     }
   }
 }

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());

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,