From 868f1bb1bea588043a7ee8a8881cb29729816713 Mon Sep 17 00:00:00 2001 From: Aditya Date: Fri, 2 Sep 2022 12:51:35 -0700 Subject: [PATCH] Ported https://github.com/gazebosim/gz-sensors/pull/256 Signed-off-by: Aditya --- src/CameraSensor.cc | 20 ++++- test/integration/camera.cc | 131 +++++++++++++++++++++++++++++++++ test/sdf/camera_intrinsics.sdf | 42 +++++++++++ 3 files changed, 190 insertions(+), 3 deletions(-) create mode 100644 test/sdf/camera_intrinsics.sdf diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 63e01db6..3b2ce40f 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -142,15 +142,13 @@ class gz::sensors::CameraSensorPrivate ////////////////////////////////////////////////// bool CameraSensor::CreateCamera() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { gzerr << "Unable to access camera SDF element.\n"; return false; } - this->PopulateInfo(cameraSdf); - unsigned int width = cameraSdf->ImageWidth(); unsigned int height = cameraSdf->ImageHeight(); @@ -238,6 +236,22 @@ bool CameraSensor::CreateCamera() this->dataPtr->saveImage = true; } + // 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(); + + cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); + } + + // Populate camera info topic + this->PopulateInfo(cameraSdf); + return true; } diff --git a/test/integration/camera.cc b/test/integration/camera.cc index fdc6c92c..81a53e0c 100644 --- a/test/integration/camera.cc +++ b/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,133 @@ TEST_P(CameraSensorTest, LInt8ImagesWithBuiltinSDF) ImageFormatLInt8LInt16(GetParam()); } +////////////////////////////////////////////////// +void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine) +{ + 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(cameraWithoutIntrinsicsTag); + auto *sensor2 = + mgr.CreateSensor(cameraWithIntrinsicsTag); + ASSERT_NE(sensor1, nullptr); + ASSERT_NE(sensor2, nullptr); + sensor1->SetScene(scene); + sensor2->SetScene(scene); + + std::string infoTopic1 = "/camera1/camera_info"; + std::string infoTopic2 = "/camera2/camera_info"; + WaitForMessageTestHelper helper1("/camera1/image"); + WaitForMessageTestHelper helper2(infoTopic1); + WaitForMessageTestHelper helper3("/camera2/image"); + WaitForMessageTestHelper 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; + unsigned int camera1Counter = 0; + unsigned int camera2Counter = 0; + + std::function camera1InfoCallback = + [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) { + camera1Info = _msg; + camera1Counter++; + }; + + std::function camera2InfoCallback = + [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) { + camera2Info = _msg; + camera2Counter++; + }; + + // 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 + bool done = false; + int sleep = 0; + int maxSleep = 10; + while (!done && sleep++ < maxSleep) + { + std::lock_guard lock(g_mutex); + done = (camera1Counter > 0 && camera2Counter > 0); + 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_EQ(camera1Info.width(), 1000u); + EXPECT_EQ(camera1Info.height(), 1000u); + 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_EQ(camera2Info.width(), 1000u); + EXPECT_EQ(camera2Info.height(), 1000u); + 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()); diff --git a/test/sdf/camera_intrinsics.sdf b/test/sdf/camera_intrinsics.sdf new file mode 100644 index 00000000..ca1f10a0 --- /dev/null +++ b/test/sdf/camera_intrinsics.sdf @@ -0,0 +1,42 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.05 + + 1000 + 1000 + L8 + + + + + 10 + base_camera + /camera2/image + + 1.05 + + 1000 + 1000 + L8 + + + + 866.23 + 866.23 + 500 + 500 + 0 + + + + + + +