Expose camera intrinsic matrix API (#700)

* Expose camera intrinsic matrix API Signed-off-by: deepanshu <[email protected]> Co-authored-by: Aditya Pande <[email protected]>
gazebosim · Sep 7, 2022 · 581aaab · 581aaab
1 parent 1694f3f
commit 581aaab
Show file tree

Hide file tree

Showing 7 changed files with 105 additions and 0 deletions.
diff --git a/include/gz/rendering/Camera.hh b/include/gz/rendering/Camera.hh
@@ -194,6 +194,13 @@ namespace gz
       /// \return A pointer to the render window.
       public: virtual RenderWindowPtr CreateRenderWindow() = 0;
 
+      /// \brief Get the camera intrinsic matrix.
+      ///        This matrix is different than the matrix returned from
+      ///        ProjectionMatrix() which is used by OpenGL internally.
+      ///        The matrix returned contains the camera calibrated values.
+      /// \return intrinsic matrix
+      public: virtual math::Matrix3d CameraIntrinsicMatrix() const = 0;
+
       /// \brief Get the projection matrix for this camera
       /// \return Camera projection matrix
       public: virtual math::Matrix4d ProjectionMatrix() const = 0;

diff --git a/include/gz/rendering/base/BaseCamera.hh b/include/gz/rendering/base/BaseCamera.hh
@@ -108,6 +108,9 @@ namespace gz
       public: virtual VisualPtr VisualAt(const gz::math::Vector2i
                   &_mousePos) override;
 
+      // Documentation inherited.
+      public: virtual math::Matrix3d CameraIntrinsicMatrix() const override;
+
       // Documentation inherited.
       public: virtual math::Matrix4d ProjectionMatrix() const override;
 
@@ -498,6 +501,26 @@ namespace gz
       return RenderWindowPtr();
     }
 
+    //////////////////////////////////////////////////
+    template <class T>
+    math::Matrix3d BaseCamera<T>::CameraIntrinsicMatrix() const
+    {
+      // Extracting the intrinsic matrix :
+      // https://ogrecave.github.io/ogre/api/13/class_ogre_1_1_math.html
+      const math::Matrix4d& projectionMat = this->ProjectionMatrix();
+
+      const double width = static_cast<double>(this->ImageWidth());
+      const double height = static_cast<double>(this->ImageHeight());
+      double fX = (projectionMat(0, 0) * width) / 2.0;
+      double fY = (projectionMat(1, 1) * height) / 2.0;
+      double cX = (-1.0 * width * (projectionMat(0, 2) - 1.0)) / 2.0;
+      double cY = height + (height * (projectionMat(1, 2) - 1)) / 2.0;
+
+      return math::Matrix3d(fX, 0, cX,
+                            0, fY, cY,
+                            0, 0, 1);
+    }
+
     //////////////////////////////////////////////////
     template <class T>
     math::Matrix4d BaseCamera<T>::ProjectionMatrix() const

diff --git a/ogre/include/gz/rendering/ogre/OgreCamera.hh b/ogre/include/gz/rendering/ogre/OgreCamera.hh
@@ -81,6 +81,9 @@ namespace gz
       // Documentation inherited.
       public: virtual RenderWindowPtr CreateRenderWindow() override;
 
+      // Documentation inherited.
+      public: virtual math::Matrix3d CameraIntrinsicMatrix() const override;
+
       // Documentation inherited.
       public: virtual math::Matrix4d ProjectionMatrix() const override;
 

diff --git a/ogre/src/OgreCamera.cc b/ogre/src/OgreCamera.cc
@@ -269,6 +269,12 @@ RenderWindowPtr OgreCamera::CreateRenderWindow()
   return base;
 }
 
+//////////////////////////////////////////////////
+math::Matrix3d OgreCamera::CameraIntrinsicMatrix() const
+{
+  return BaseCamera::CameraIntrinsicMatrix();
+}
+
 //////////////////////////////////////////////////
 math::Matrix4d OgreCamera::ProjectionMatrix() const
 {

diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2Camera.hh b/ogre2/include/gz/rendering/ogre2/Ogre2Camera.hh
@@ -93,6 +93,9 @@ namespace gz
       // Documentation inherited.
       public: virtual RenderWindowPtr CreateRenderWindow() override;
 
+      // Documentation inherited.
+      public: virtual math::Matrix3d CameraIntrinsicMatrix() const override;
+
       // Documentation inherited.
       public: virtual math::Matrix4d ProjectionMatrix() const override;
 

diff --git a/ogre2/src/Ogre2Camera.cc b/ogre2/src/Ogre2Camera.cc
@@ -317,6 +317,12 @@ RenderWindowPtr Ogre2Camera::CreateRenderWindow()
   return RenderWindowPtr();
 }
 
+//////////////////////////////////////////////////
+math::Matrix3d Ogre2Camera::CameraIntrinsicMatrix() const
+{
+  return BaseCamera::CameraIntrinsicMatrix();
+}
+
 //////////////////////////////////////////////////
 math::Matrix4d Ogre2Camera::ProjectionMatrix() const
 {

diff --git a/test/common_test/Camera_TEST.cc b/test/common_test/Camera_TEST.cc
@@ -298,3 +298,60 @@ 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 = camera->CameraIntrinsicMatrix();
+  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 = camera->CameraIntrinsicMatrix();
+  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);
+}