ogre2: Set custom projection matrix for other types of cameras

ogre2/src/Ogre2BoundingBoxCamera.cc

@@ -31,6 +31,7 @@
 #include <gz/math/Color.hh>
 #include <gz/math/Vector4.hh>
 #include <gz/math/eigen3/Util.hh>
+#include <gz/math/Matrix4.hh>
 #include <gz/math/OrientedBox.hh>
 
 #include "gz/rendering/RenderTypes.hh"
@@ -464,6 +465,21 @@ void Ogre2BoundingBoxCamera::PreRender()
   if (!this->dataPtr->ogreRenderTexture)
     this->CreateBoundingBoxTexture();
 
+  // todo(iche033) Override BaseCamera::SetProjectionMatrix() function in
+  // main / gz-rendering9 instead of checking and setting the custom
+  // projection matrix here
+  if (this->dataPtr->ogreCamera &&
+      this->projectionMatrix != gz::math::Matrix4d::Zero)
+  {
+    if (this->projectionMatrix !=
+        Ogre2Conversions::Convert(
+        this->dataPtr->ogreCamera->getProjectionMatrix()))
+    {
+      this->dataPtr->ogreCamera->setCustomProjectionMatrix(true,
+          Ogre2Conversions::Convert(this->projectionMatrix));
+    }
+  }
+
   this->dataPtr->outputBoxes.clear();
 }
 

ogre2/src/Ogre2DepthCamera.cc

@@ -26,6 +26,7 @@
 #include <cstdint>
 #include <math.h>
 #include <gz/math/Helpers.hh>
+#include <gz/math/Matrix4.hh>
 
 #include "gz/rendering/RenderTypes.hh"
 #include "gz/rendering/ogre2/Ogre2Conversions.hh"
@@ -1056,6 +1057,19 @@ void Ogre2DepthCamera::PreRender()
   if (!this->dataPtr->ogreCompositorWorkspace)
     this->CreateWorkspaceInstance();
 
+  // todo(iche033) Override BaseCamera::SetProjectionMatrix() function in
+  // main / gz-rendering9 instead of checking and setting the custom
+  // projection matrix here
+  if (this->ogreCamera &&
+      this->projectionMatrix != gz::math::Matrix4d::Zero)
+  {
+    if (this->projectionMatrix !=
+        Ogre2Conversions::Convert(this->ogreCamera->getProjectionMatrix()))
+    {
+      this->ogreCamera->setCustomProjectionMatrix(true,
+          Ogre2Conversions::Convert(this->projectionMatrix));
+    }
+  }
 
   // Disable color target (set to clear pass) if there are no rgb point cloud
   // connections

ogre2/src/Ogre2SegmentationCamera.cc

@@ -19,6 +19,7 @@
 
 #include <gz/common/Console.hh>
 #include <gz/math/Color.hh>
+#include <gz/math/Matrix4.hh>
 
 #include "gz/rendering/ogre2/Ogre2Camera.hh"
 #include "gz/rendering/ogre2/Ogre2Conversions.hh"
@@ -164,6 +165,20 @@ void Ogre2SegmentationCamera::PreRender()
 {
   if (!this->dataPtr->ogreSegmentationTexture)
     this->CreateSegmentationTexture();
+
+  // todo(iche033) Override BaseCamera::SetProjectionMatrix() function in
+  // main / gz-rendering9 instead of checking and setting the custom
+  // projection matrix here
+  if (this->ogreCamera &&
+      this->projectionMatrix != gz::math::Matrix4d::Zero)
+  {
+    if (this->projectionMatrix !=
+        Ogre2Conversions::Convert(this->ogreCamera->getProjectionMatrix()))
+    {
+      this->ogreCamera->setCustomProjectionMatrix(true,
+          Ogre2Conversions::Convert(this->projectionMatrix));
+    }
+  }
 }
 
 /////////////////////////////////////////////////

ogre2/src/Ogre2ThermalCamera.cc

@@ -43,6 +43,7 @@
 #include <gz/common/Console.hh>
 #include <gz/common/Filesystem.hh>
 #include <gz/math/Helpers.hh>
+#include <gz/math/Matrix4.hh>
 
 #include "gz/rendering/RenderTypes.hh"
 #include "gz/rendering/ogre2/Ogre2Conversions.hh"
@@ -1130,6 +1131,20 @@ void Ogre2ThermalCamera::PreRender()
 {
   if (!this->dataPtr->ogreThermalTexture)
     this->CreateThermalTexture();
+
+  // todo(iche033) Override BaseCamera::SetProjectionMatrix() function in
+  // main / gz-rendering9 instead of checking and setting the custom
+  // projection matrix here
+  if (this->ogreCamera &&
+      this->projectionMatrix != gz::math::Matrix4d::Zero)
+  {
+    if (this->projectionMatrix !=
+        Ogre2Conversions::Convert(this->ogreCamera->getProjectionMatrix()))
+    {
+      this->ogreCamera->setCustomProjectionMatrix(true,
+          Ogre2Conversions::Convert(this->projectionMatrix));
+    }
+  }
 }
 
 //////////////////////////////////////////////////

test/integration/depth_camera.cc

@@ -16,6 +16,7 @@
 */
 
 #include <gtest/gtest.h>
+#include <string>
 
 #include "CommonRenderingTest.hh"
 
@@ -735,3 +736,123 @@ TEST_F(DepthCameraTest, DepthCameraParticles)
 
   engine->DestroyScene(scene);
 }
+
+/////////////////////////////////////////////////
+TEST_F(DepthCameraTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(DepthCameraProjection))
+{
+  CHECK_SUPPORTED_ENGINE("ogre2");
+
+  int imgWidth = 256;
+  int imgHeight = 256;
+  double aspectRatio = imgWidth / imgHeight;
+
+  double unitBoxSize = 1.0;
+  gz::math::Vector3d boxPosition(1.8, 0.0, 0.0);
+
+  gz::rendering::ScenePtr scene = engine->CreateScene("scene");
+  ASSERT_NE(nullptr, scene);
+
+  // Create an scene with a box in it
+  gz::rendering::VisualPtr root = scene->RootVisual();
+
+  // create box visual
+  gz::rendering::VisualPtr box = scene->CreateVisual();
+  box->AddGeometry(scene->CreateBox());
+  box->SetLocalPosition(boxPosition);
+  box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize);
+  root->AddChild(box);
+  {
+    // Create depth camera
+    auto depthCamera = scene->CreateDepthCamera("DepthCamera");
+    ASSERT_NE(depthCamera, nullptr);
+
+    gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0),
+        gz::math::Quaterniond::Identity);
+    depthCamera->SetLocalPose(testPose);
+
+    // Set initial camera parameters using a wide horizontal FOV
+    double farDist = 100.0;
+    double nearDist = 0.01;
+    double hfov = 1.5;
+    depthCamera->SetImageWidth(imgWidth);
+    EXPECT_EQ(depthCamera->ImageWidth(),
+      static_cast<unsigned int>(imgWidth));
+    depthCamera->SetImageHeight(imgHeight);
+    EXPECT_EQ(depthCamera->ImageHeight(),
+      static_cast<unsigned int>(imgHeight));
+    depthCamera->SetFarClipPlane(farDist);
+    EXPECT_DOUBLE_EQ(depthCamera->FarClipPlane(), farDist);
+    depthCamera->SetNearClipPlane(nearDist);
+    EXPECT_DOUBLE_EQ(depthCamera->NearClipPlane(), nearDist);
+    depthCamera->SetAspectRatio(aspectRatio);
+    EXPECT_DOUBLE_EQ(depthCamera->AspectRatio(), aspectRatio);
+    depthCamera->SetHFOV(hfov);
+    EXPECT_DOUBLE_EQ(depthCamera->HFOV().Radian(), hfov);
+
+    depthCamera->CreateDepthTexture();
+    scene->RootVisual()->AddChild(depthCamera);
+
+    // Set a callback on the  camera sensor to get a depth camera frame
+    float *scan = new float[imgHeight * imgWidth];
+    gz::common::ConnectionPtr connection =
+      depthCamera->ConnectNewDepthFrame(
+          std::bind(&::OnNewDepthFrame, scan,
+            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+            std::placeholders::_4, std::placeholders::_5));
+
+    g_depthCounter = 0u;
+    depthCamera->Update();
+    scene->SetTime(scene->Time() + std::chrono::milliseconds(16));
+    EXPECT_EQ(1u, g_depthCounter);
+
+    float expectedRange = boxPosition.X() - unitBoxSize * 0.5;
+    unsigned int hasInfValues = 0u;
+    unsigned int hasBoxValues = 0u;
+    for (unsigned int i = 0; i < depthCamera->ImageHeight(); ++i)
+    {
+      for (unsigned int j = 0; j < depthCamera->ImageWidth(); ++j)
+      {
+        float x = scan[i * depthCamera->ImageWidth() + j];
+        if (gz::math::equal(expectedRange, x))
+          hasBoxValues++;
+        else if (std::isinf(x))
+          hasInfValues++;
+        else
+          FAIL() << "Unexpected range value: " << x;
+      }
+    }
+    EXPECT_LT(0u, hasBoxValues);
+    EXPECT_LT(0u, hasInfValues);
+
+    // Now override with a custom projection matrix
+    // This projection matrix corresponds to a small horizontal FOV
+    // (hfov = 0.5)
+    gz::math::Matrix4d projectionMatrix(
+        3.91632, 0, 0, 0,
+        0, 3.91632, 0, 0,
+        0, 0, -1.0002, -0.20002,
+        0, 0, -1, 0);
+    depthCamera->SetProjectionMatrix(projectionMatrix);
+    depthCamera->Update();
+    scene->SetTime(scene->Time() + std::chrono::milliseconds(16));
+    EXPECT_EQ(2u, g_depthCounter);
+
+    // The camera should use the updated projection matrix and
+    // the box should fill the whole image.
+    // Verify all range values at the expected box range
+    for (unsigned int i = 0; i < depthCamera->ImageHeight(); ++i)
+    {
+      for (unsigned int j = 0; j < depthCamera->ImageWidth(); ++j)
+      {
+        float x = scan[i * depthCamera->ImageWidth() + j];
+        EXPECT_FLOAT_EQ(expectedRange, x);
+       }
+    }
+
+    // Clean up
+    connection.reset();
+    delete [] scan;
+  }
+
+  engine->DestroyScene(scene);
+}