Calculate camera intrinsics : Refactor #700 (#755)

* Added free function to get camera intrinsics
Signed-off-by: Aditya <[email protected]>

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

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,

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