From ccfa340f5509beb6a544b520abc17bdf1723faca Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 May 2024 06:33:14 +0000 Subject: [PATCH] set camera projection based on sdf params Signed-off-by: Ian Chen --- test/integration/depth_camera.cc | 387 +++++++++++++++++++++++++++ test/sdf/depth_camera_intrinsics.sdf | 77 ++++++ test/sdf/depth_camera_projection.sdf | 70 +++++ 3 files changed, 534 insertions(+) create mode 100644 test/sdf/depth_camera_intrinsics.sdf create mode 100644 test/sdf/depth_camera_projection.sdf diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index e47f40ad..a926bac4 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -154,6 +154,12 @@ class DepthCameraSensorTest: public testing::Test, } // Create a Camera sensor from a SDF and gets a image message public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); + + // Create depth camera sensors and verify camera intrinsics + public: void DepthCameraIntrinsics(const std::string &_renderEngine); + + // Create depth camera sensors and verify camera projection + public: void DepthCameraProjection(const std::string &_renderEngine); }; void DepthCameraSensorTest::ImagesWithBuiltinSDF( @@ -514,5 +520,386 @@ TEST_P(DepthCameraSensorTest, ImagesWithBuiltinSDF) gz::common::Console::SetVerbosity(4); } +////////////////////////////////////////////////// +void DepthCameraSensorTest::DepthCameraIntrinsics( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "depth_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(); + + // Camera sensor with different intrinsics tag + auto cameraWithDiffIntrinsicsTag = + cameraWithIntrinsicsTag->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"); + gz::rendering::VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create box visual + gz::rendering::VisualPtr box = scene->CreateVisual("box"); + ASSERT_NE(nullptr, box); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5)); + box->SetLocalRotation(0, 0, 0); + root->AddChild(box); + + // Do the test + gz::sensors::Manager mgr; + + // there are 3 cameras: + // - camera1: no intrinsics params + // - camera2: has intrinsics params that are the same as default values + // - camera3: has intrinsics params that are different from default values + // camera1 and camera2 should produce very similar images and camera3 should + // produce different images from 1 and 2. + auto *sensor1 = mgr.CreateSensor( + cameraWithoutIntrinsicsTag); + auto *sensor2 = mgr.CreateSensor( + cameraWithIntrinsicsTag); + auto *sensor3 = mgr.CreateSensor( + cameraWithDiffIntrinsicsTag); + + ASSERT_NE(sensor1, nullptr); + ASSERT_NE(sensor2, nullptr); + ASSERT_NE(sensor3, nullptr); + sensor1->SetScene(scene); + sensor2->SetScene(scene); + sensor3->SetScene(scene); + + std::string infoTopic1 = "/camera1/camera_info"; + std::string infoTopic2 = "/camera2/camera_info"; + std::string infoTopic3 = "/camera3/camera_info"; + std::string imgTopic1 = "/camera1/image"; + std::string imgTopic2 = "/camera2/image"; + std::string imgTopic3 = "/camera3/image"; + WaitForMessageTestHelper helper1(imgTopic1); + WaitForMessageTestHelper helper2(infoTopic1); + WaitForMessageTestHelper helper3(imgTopic2); + WaitForMessageTestHelper helper4(infoTopic2); + WaitForMessageTestHelper helper5(imgTopic3); + WaitForMessageTestHelper helper6(infoTopic3); + + EXPECT_TRUE(sensor1->HasConnections()); + EXPECT_TRUE(sensor2->HasConnections()); + EXPECT_TRUE(sensor3->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; + EXPECT_TRUE(helper5.WaitForMessage()) << helper5; + EXPECT_TRUE(helper6.WaitForMessage()) << helper6; + + // Subscribe to the camera info topic + gz::msgs::CameraInfo camera1Info, camera2Info, camera3Info; + unsigned int camera1Counter = 0u; + unsigned int camera2Counter = 0u; + unsigned int camera3Counter = 0u; + + 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++; + }; + std::function camera3InfoCallback = + [&camera3Info, &camera3Counter](const gz::msgs::CameraInfo& _msg) + { + camera3Info = _msg; + camera3Counter++; + }; + + unsigned int height = 1000u; + unsigned int width = 1000u; + // Subscribe to the camera topic + gz::transport::Node node; + node.Subscribe(infoTopic1, camera1InfoCallback); + node.Subscribe(infoTopic2, camera2InfoCallback); + node.Subscribe(infoTopic3, camera3InfoCallback); + + // 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 > 0u && camera2Counter > 0u && camera3Counter > 0u); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Camera sensor without intrinsics tag + double error = 3e-1; + EXPECT_EQ(camera1Info.width(), width); + EXPECT_EQ(camera1Info.height(), height); + EXPECT_NEAR(camera1Info.intrinsics().k(0), 866.23, error); + EXPECT_NEAR(camera1Info.intrinsics().k(4), 866.23, 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(), width); + EXPECT_EQ(camera2Info.height(), height); + 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); + + // Camera sensor with different intrinsics tag + EXPECT_EQ(camera3Info.width(), width); + EXPECT_EQ(camera3Info.height(), height); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(0), 900); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(4), 900); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(2), 501); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(5), 501); + + // Clean up rendering ptrs + box.reset(); + + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); + mgr.Remove(sensor3->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(DepthCameraSensorTest, CameraIntrinsics) +{ + gz::common::Console::SetVerbosity(2); + DepthCameraIntrinsics(GetParam()); +} + +////////////////////////////////////////////////// +void DepthCameraSensorTest::DepthCameraProjection( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "depth_camera_projection.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 projection tag + auto cameraWithoutProjectionsTag = linkPtr->GetElement("sensor"); + + // Camera sensor with projection tag + auto cameraWithProjectionsTag = + linkPtr->GetElement("sensor")->GetNextElement(); + + // Camera sensor with different projection tag + auto cameraWithDiffProjectionsTag = + cameraWithProjectionsTag->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"); + gz::rendering::VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create box visual + gz::rendering::VisualPtr box = scene->CreateVisual("box"); + ASSERT_NE(nullptr, box); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5)); + box->SetLocalRotation(0, 0, 0); + root->AddChild(box); + + // Do the test + gz::sensors::Manager mgr; + + auto *sensor1 = mgr.CreateSensor( + cameraWithoutProjectionsTag); + auto *sensor2 = mgr.CreateSensor( + cameraWithProjectionsTag); + auto *sensor3 = mgr.CreateSensor( + cameraWithDiffProjectionsTag); + + ASSERT_NE(sensor1, nullptr); + ASSERT_NE(sensor2, nullptr); + ASSERT_NE(sensor3, nullptr); + std::string imgTopic1 = "/camera1/image"; + std::string imgTopic2 = "/camera2/image"; + std::string imgTopic3 = "/camera3/image"; + sensor1->SetScene(scene); + sensor2->SetScene(scene); + sensor3->SetScene(scene); + + std::string infoTopic1 = "/camera1/camera_info"; + std::string infoTopic2 = "/camera2/camera_info"; + std::string infoTopic3 = "/camera3/camera_info"; + WaitForMessageTestHelper helper1("/camera1/image"); + WaitForMessageTestHelper helper2(infoTopic1); + WaitForMessageTestHelper helper3("/camera2/image"); + WaitForMessageTestHelper helper4(infoTopic2); + WaitForMessageTestHelper helper5(imgTopic3); + WaitForMessageTestHelper helper6(infoTopic3); + + EXPECT_TRUE(sensor1->HasConnections()); + EXPECT_TRUE(sensor2->HasConnections()); + EXPECT_TRUE(sensor3->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; + EXPECT_TRUE(helper5.WaitForMessage()) << helper5; + EXPECT_TRUE(helper6.WaitForMessage()) << helper6; + + // Subscribe to the camera info topic + gz::msgs::CameraInfo camera1Info, camera2Info, camera3Info; + unsigned int camera1Counter = 0u; + unsigned int camera2Counter = 0u; + unsigned int camera3Counter = 0u; + + 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++; + }; + std::function camera3InfoCallback = + [&camera3Info, &camera3Counter](const gz::msgs::CameraInfo& _msg) + { + camera3Info = _msg; + camera3Counter++; + }; + + unsigned int height = 1000u; + unsigned int width = 1000u; + + // Subscribe to the camera topic + gz::transport::Node node; + node.Subscribe(infoTopic1, camera1InfoCallback); + node.Subscribe(infoTopic2, camera2InfoCallback); + node.Subscribe(infoTopic3, camera3InfoCallback); + + // 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 > 0u && camera2Counter > 0u && camera3Counter > 0u); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Camera sensor without projection tag + // account for error converting gl projection values back to + // cv projection values + double error = 1.0; + EXPECT_EQ(camera1Info.width(), width); + EXPECT_EQ(camera1Info.height(), height); + EXPECT_NEAR(camera1Info.projection().p(0), 866.23, error); + EXPECT_NEAR(camera1Info.projection().p(5), 866.23, error); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0.0); + + // Camera sensor with projection tag + EXPECT_EQ(camera2Info.width(), width); + EXPECT_EQ(camera2Info.height(), height); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(0), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(5), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200.0); + + // Camera sensor with different projection tag + EXPECT_EQ(camera3Info.width(), width); + EXPECT_EQ(camera3Info.height(), height); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(0), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0.0); + + // Clean up rendering ptrs + box.reset(); + + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); + mgr.Remove(sensor3->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(DepthCameraSensorTest, CameraProjection) +{ + gz::common::Console::SetVerbosity(2); + DepthCameraProjection(GetParam()); +} + INSTANTIATE_TEST_SUITE_P(DepthCameraSensor, DepthCameraSensorTest, RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); diff --git a/test/sdf/depth_camera_intrinsics.sdf b/test/sdf/depth_camera_intrinsics.sdf new file mode 100644 index 00000000..ae8333ab --- /dev/null +++ b/test/sdf/depth_camera_intrinsics.sdf @@ -0,0 +1,77 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 866.23 + 866.23 + 500 + 500 + 0 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + + + + + + + + diff --git a/test/sdf/depth_camera_projection.sdf b/test/sdf/depth_camera_projection.sdf new file mode 100644 index 00000000..1f994ca9 --- /dev/null +++ b/test/sdf/depth_camera_projection.sdf @@ -0,0 +1,70 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + + 866.23 + 866.23 + 500 + 500 + 300 + 200 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + 0 + + + + + + +