Update Camera Intrinsics in camera_info topic

src/CameraSensor.cc

@@ -149,8 +149,6 @@ bool CameraSensor::CreateCamera()
     return false;
   }
 
-  this->PopulateInfo(cameraSdf);
-
   unsigned int width = cameraSdf->ImageWidth();
   unsigned int height = cameraSdf->ImageHeight();
 
@@ -238,6 +236,24 @@ bool CameraSensor::CreateCamera()
     this->dataPtr->saveImage = true;
   }
 
+  auto *cameraSdfCopy = const_cast<sdf::Camera *>(cameraSdf);
+
+  // update the DOM object intrinsics to have consistent
+  // intrinsics between ogre camera and camera_info msg
+  if(!cameraSdf->HasLensIntrinsics())
+  {
+    const math::Matrix3d& intrinsicMatrix =
+        this->dataPtr->camera->CameraIntrinsicMatrix();
+
+    cameraSdfCopy->SetLensIntrinsicsFx(intrinsicMatrix(0, 0));
+    cameraSdfCopy->SetLensIntrinsicsFy(intrinsicMatrix(1, 1));
+    cameraSdfCopy->SetLensIntrinsicsCx(intrinsicMatrix(0, 2));
+    cameraSdfCopy->SetLensIntrinsicsCy(intrinsicMatrix(1, 2));
+  }
+
+  // Populate camera info topic
+  this->PopulateInfo(cameraSdfCopy);
+
   return true;
 }
 

test/integration/camera.cc

@@ -84,6 +84,9 @@ class CameraSensorTest: public testing::Test,
 
   // Create 8 bit and 16 bit grayscale camera sensors and verify image format
   public: void ImageFormatLInt8LInt16(const std::string &_renderEngine);
+
+  // Create camera sensors and verify camera intrinsics
+  public: void CameraIntrinsics(const std::string &_renderEngine);
 };
 
 void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine)
@@ -293,5 +296,130 @@ TEST_P(CameraSensorTest, LInt8ImagesWithBuiltinSDF)
   ImageFormatLInt8LInt16(GetParam());
 }
 
+//////////////////////////////////////////////////
+void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine)
+{
+  // Get the darn test data
+  std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test",
+                                           "sdf", "camera_intrinsics.sdf");
+  sdf::SDFPtr doc(new sdf::SDF());
+  sdf::init(doc);
+  ASSERT_TRUE(sdf::readFile(path, doc));
+  ASSERT_NE(nullptr, doc->Root());
+  ASSERT_TRUE(doc->Root()->HasElement("model"));
+  auto modelPtr = doc->Root()->GetElement("model");
+  ASSERT_TRUE(modelPtr->HasElement("link"));
+  auto linkPtr = modelPtr->GetElement("link");
+  ASSERT_TRUE(linkPtr->HasElement("sensor"));
+
+  // Camera sensor without intrinsics tag
+  auto cameraWithoutIntrinsicsTag = linkPtr->GetElement("sensor");
+
+  // Camera sensor with intrinsics tag
+  auto cameraWithIntrinsicsTag =
+      linkPtr->GetElement("sensor")->GetNextElement();
+
+  // Setup gz-rendering with an empty scene
+  auto *engine = gz::rendering::engine(_renderEngine);
+  if (!engine)
+  {
+    gzdbg << "Engine '" << _renderEngine
+          << "' is not supported" << std::endl;
+    return;
+  }
+
+  gz::rendering::ScenePtr scene = engine->CreateScene("scene");
+
+  // Do the test
+  gz::sensors::Manager mgr;
+
+  auto *sensor1 =
+      mgr.CreateSensor<gz::sensors::CameraSensor>(cameraWithoutIntrinsicsTag);
+  auto *sensor2 =
+      mgr.CreateSensor<gz::sensors::CameraSensor>(cameraWithIntrinsicsTag);
+  ASSERT_NE(sensor1, nullptr);
+  ASSERT_NE(sensor2, nullptr);
+  sensor1->SetScene(scene);
+  sensor2->SetScene(scene);
+
+  std::string imageTopic1 = "/camera1/image";
+  std::string infoTopic1 = "/camera1/camera_info";
+  std::string imageTopic2 = "/camera2/image";
+  std::string infoTopic2 = "/camera2/camera_info";
+  WaitForMessageTestHelper<gz::msgs::Image> helper1(imageTopic1);
+  WaitForMessageTestHelper<gz::msgs::CameraInfo> helper2(infoTopic1);
+  WaitForMessageTestHelper<gz::msgs::Image> helper3(imageTopic2);
+  WaitForMessageTestHelper<gz::msgs::CameraInfo> helper4(infoTopic2);
+  EXPECT_TRUE(sensor1->HasConnections());
+  EXPECT_TRUE(sensor2->HasConnections());
+
+  // Update once to create image
+  mgr.RunOnce(std::chrono::steady_clock::duration::zero());
+
+  EXPECT_TRUE(helper1.WaitForMessage()) << helper1;
+  EXPECT_TRUE(helper2.WaitForMessage()) << helper2;
+  EXPECT_TRUE(helper3.WaitForMessage()) << helper3;
+  EXPECT_TRUE(helper4.WaitForMessage()) << helper4;
+
+  // Subscribe to the camera info topic
+  gz::msgs::CameraInfo camera1Info, camera2Info;
+
+  std::function<void(const gz::msgs::CameraInfo&)> camera1InfoCallback =
+      [&camera1Info](const gz::msgs::CameraInfo& _msg) {
+        camera1Info = _msg;
+  };
+
+  std::function<void(const gz::msgs::CameraInfo&)> camera2InfoCallback =
+      [&camera2Info](const gz::msgs::CameraInfo& _msg) {
+        camera2Info = _msg;
+  };
+
+  // Subscribe to the camera topic
+  gz::transport::Node node;
+  node.Subscribe(infoTopic1, camera1InfoCallback);
+  node.Subscribe(infoTopic2, camera2InfoCallback);
+
+  // Wait for a few camera frames
+  mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true);
+
+  // Run to get image and check image format in callback
+  int sleep = 0;
+  int maxSleep = 10;
+  while (sleep++ < maxSleep)
+  {
+    std::lock_guard<std::mutex> lock(g_mutex);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  // Image size, focal length and optical center
+  // Camera sensor without intrinsics tag
+  double error = 1e-1;
+  EXPECT_DOUBLE_EQ(camera1Info.width(), 1000);
+  EXPECT_DOUBLE_EQ(camera1Info.height(), 1000);
+  EXPECT_NEAR(camera1Info.intrinsics().k(0), 863.2297, error);
+  EXPECT_NEAR(camera1Info.intrinsics().k(4), 863.2297, error);
+  EXPECT_DOUBLE_EQ(camera1Info.intrinsics().k(2), 500);
+  EXPECT_DOUBLE_EQ(camera1Info.intrinsics().k(5), 500);
+
+  // Camera sensor with intrinsics tag
+  EXPECT_DOUBLE_EQ(camera2Info.width(), 1000);
+  EXPECT_DOUBLE_EQ(camera2Info.height(), 1000);
+  EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(0), 866.23);
+  EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(4), 866.23);
+  EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(2), 500);
+  EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(5), 500);
+
+  // Clean up
+  engine->DestroyScene(scene);
+  gz::rendering::unloadEngine(engine->Name());
+}
+
+//////////////////////////////////////////////////
+TEST_P(CameraSensorTest, CameraIntrinsics)
+{
+  gz::common::Console::SetVerbosity(2);
+  CameraIntrinsics(GetParam());
+}
+
 INSTANTIATE_TEST_SUITE_P(CameraSensor, CameraSensorTest,
     RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam());

test/sdf/camera_intrinsics.sdf

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="m1">
+    <link name="link1">
+      <sensor name="camera_without_intrinsics_tag" type="camera">
+        <update_rate>10</update_rate>
+        <ignition_frame_id>base_camera</ignition_frame_id>
+        <topic>/camera1/image</topic>
+        <camera>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>1000</width>
+            <height>1000</height>
+            <format>L8</format>
+          </image>
+        </camera>
+      </sensor>
+      <sensor name="camera_with_intrinsics_tag" type="camera">
+        <update_rate>10</update_rate>
+        <ignition_frame_id>base_camera</ignition_frame_id>
+        <topic>/camera2/image</topic>
+        <camera>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>1000</width>
+            <height>1000</height>
+            <format>L8</format>
+          </image>
+          <lens>
+            <intrinsics>
+              <fx>866.23</fx>
+              <fy>866.23</fy>
+              <cx>500</cx>
+              <cy>500</cy>
+              <s>0</s>
+            </intrinsics>
+          </lens>
+        </camera>
+      </sensor>
+    </link>
+  </model>
+</sdf>