Expose camera intrinsic matrix API

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;

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,32 @@ namespace gz
       return RenderWindowPtr();
     }
 
+    //////////////////////////////////////////////////
+    template <class T>
+    math::Matrix3d BaseCamera<T>::CameraIntrinsicMatrix() const
+    {
+      const math::Matrix4d& projectionMat = this->ProjectionMatrix();
+
+      double right = this->ImageWidth();
+      double left = 0.0;
+      double top = this->ImageHeight();
+      double bottom = 0.0;
+
+      double inverseWidth = 1.0 / (right - left);
+      double inverseHeight = 1.0 / (top - bottom);
+
+      double fX = projectionMat(0, 0)/ (2*inverseWidth);
+      double fY = projectionMat(1, 1) / (2*inverseHeight);
+      double cX = -((projectionMat(0, 2) -
+          ((right + left) / (right - left))) * (right - left)) / 2;
+      double cY = this->ImageHeight() + ((projectionMat(1, 2) -
+          ((top + bottom) / (top - bottom))) * (top - bottom)) / 2;
+
+      return math::Matrix3d(fX, 0, cX,
+                            0, fY, cY,
+                            0, 0, 1);
+    }
+
     //////////////////////////////////////////////////
     template <class T>
     math::Matrix4d BaseCamera<T>::ProjectionMatrix() const

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;
 

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
 {

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;
 
@@ -179,7 +182,7 @@ namespace gz
 
       /// \brief Make scene our friend so it can create a camera
       private: friend class Ogre2Scene;
-
+\
       /// \brief Make ray query our friend so it can use the internal ogre
       /// camera to execute queries
       private: friend class Ogre2RayQuery;

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
 {

test/common_test/Camera_TEST.cc

@@ -298,3 +298,52 @@ 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 height = 320;
+  unsigned int width = 240;
+  double hfov = 1.047;
+
+  camera->SetImageHeight(height);
+  camera->SetImageWidth(width);
+  camera->SetHFOV(hfov);
+
+  double error = 1e-1;
+  EXPECT_DOUBLE_EQ(camera->ImageHeight(), height);
+  EXPECT_DOUBLE_EQ(camera->ImageWidth(), width);
+  EXPECT_NEAR(camera->HFOV().Radian(), hfov, error);
+
+  // Verify 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 that changing camera size changes intrinsics
+  height = 1000;
+  width = 1000;
+  camera->SetImageHeight(height);
+  camera->SetImageWidth(width);
+
+  EXPECT_DOUBLE_EQ(camera->ImageHeight(), height);
+  EXPECT_DOUBLE_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);
+}