diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index 108af8ce..cc2f5a9f 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/include/gz/sensors/CameraSensor.hh @@ -148,6 +148,17 @@ namespace gz /// \return The camera optical frame public: const std::string& OpticalFrameId() const; + /// \brief Set camera lens intrinsics and projection based on + /// values from SDF. If the camera SDF does not contain intrinsic or + /// projection parameters, the camera will not be updated. Instead, the + /// camera SDF will be updated with intrinsic and projection values + /// computed manually from current camera intrinsic properties. + /// \param[in] _camera Camera to set intrinsic and projection params. + /// \param[in,out] _cameraSdf Camera sdf with intrinisc and projection + /// parameters. + public: void UpdateLensIntrinsicsAndProjection( + rendering::CameraPtr _camera, sdf::Camera &_cameraSdf); + /// \brief Advertise camera info topic. /// \return True if successful. protected: bool AdvertiseInfo(); diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 08467b22..cc2444f9 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -280,9 +280,6 @@ bool BoundingBoxCameraSensor::CreateCamera() return false; } - // Camera Info Msg - this->PopulateInfo(sdfCamera); - if (!this->dataPtr->rgbCamera) { // Create rendering camera @@ -333,6 +330,14 @@ bool BoundingBoxCameraSensor::CreateCamera() this->AddSensor(this->dataPtr->boundingboxCamera); this->AddSensor(this->dataPtr->rgbCamera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera, + *sdfCamera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->boundingboxCamera, + *sdfCamera); + + // Camera Info Msg + this->PopulateInfo(sdfCamera); + // Create the directory to store frames if (sdfCamera->SaveFrames()) { diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 0a3237f1..c9d821bc 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -291,38 +291,8 @@ bool CameraSensor::CreateCamera() break; } - // Update the DOM object intrinsics to have consistent - // intrinsics between ogre camera and camera_info msg - if(!cameraSdf->HasLensIntrinsics()) - { - auto intrinsicMatrix = - gz::rendering::projectionToCameraIntrinsic( - this->dataPtr->camera->ProjectionMatrix(), - this->dataPtr->camera->ImageWidth(), - this->dataPtr->camera->ImageHeight() - ); - - cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); - cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); - cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); - cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); - } - // set custom projection matrix based on intrinsics param specified in sdf - else - { - double fx = cameraSdf->LensIntrinsicsFx(); - double fy = cameraSdf->LensIntrinsicsFy(); - double cx = cameraSdf->LensIntrinsicsCx(); - double cy = cameraSdf->LensIntrinsicsCy(); - double s = cameraSdf->LensIntrinsicsSkew(); - auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( - this->dataPtr->camera->ImageWidth(), - this->dataPtr->camera->ImageHeight(), - fx, fy, cx, cy, s, - this->dataPtr->camera->NearClipPlane(), - this->dataPtr->camera->FarClipPlane()); - this->dataPtr->camera->SetProjectionMatrix(projectionMatrix); - } + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera, + *cameraSdf); this->dataPtr->image = this->dataPtr->camera->CreateImage(); @@ -336,48 +306,6 @@ bool CameraSensor::CreateCamera() this->dataPtr->saveImage = true; } - // Update the DOM object intrinsics to have consistent - // projection matrix values between ogre camera and camera_info msg - // If these values are not defined in the SDF then we need to update - // these values to something reasonable. The projection matrix is - // the cumulative effect of intrinsic and extrinsic parameters - if(!cameraSdf->HasLensProjection()) - { - // Note that the matrix from Ogre via camera->ProjectionMatrix() has a - // different format than the projection matrix used in SDFormat. - // This is why they are converted using projectionToCameraIntrinsic. - // The resulting matrix is the intrinsic matrix, but since the user has - // not overridden the values, this is also equal to the projection matrix. - auto intrinsicMatrix = - gz::rendering::projectionToCameraIntrinsic( - this->dataPtr->camera->ProjectionMatrix(), - this->dataPtr->camera->ImageWidth(), - this->dataPtr->camera->ImageHeight() - ); - cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0)); - cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1)); - cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2)); - cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2)); - } - // set custom projection matrix based on projection param specified in sdf - else - { - // tx and ty are not used - double fx = cameraSdf->LensProjectionFx(); - double fy = cameraSdf->LensProjectionFy(); - double cx = cameraSdf->LensProjectionCx(); - double cy = cameraSdf->LensProjectionCy(); - double s = 0; - - auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( - this->dataPtr->camera->ImageWidth(), - this->dataPtr->camera->ImageHeight(), - fx, fy, cx, cy, s, - this->dataPtr->camera->NearClipPlane(), - this->dataPtr->camera->FarClipPlane()); - this->dataPtr->camera->SetProjectionMatrix(projectionMatrix); - } - // Populate camera info topic this->PopulateInfo(cameraSdf); @@ -886,6 +814,86 @@ const std::string& CameraSensor::OpticalFrameId() const return this->dataPtr->opticalFrameId; } +////////////////////////////////////////////////// +void CameraSensor::UpdateLensIntrinsicsAndProjection( + rendering::CameraPtr _camera, sdf::Camera &_cameraSdf) +{ + // Update the DOM object intrinsics to have consistent + // intrinsics between ogre camera and camera_info msg + if(!_cameraSdf.HasLensIntrinsics()) + { + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + _camera->ProjectionMatrix(), + _camera->ImageWidth(), + _camera->ImageHeight() + ); + + _cameraSdf.SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); + _cameraSdf.SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); + _cameraSdf.SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); + _cameraSdf.SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); + } + // set custom projection matrix based on intrinsics param specified in sdf + else + { + double fx = _cameraSdf.LensIntrinsicsFx(); + double fy = _cameraSdf.LensIntrinsicsFy(); + double cx = _cameraSdf.LensIntrinsicsCx(); + double cy = _cameraSdf.LensIntrinsicsCy(); + double s = _cameraSdf.LensIntrinsicsSkew(); + auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + _camera->ImageWidth(), + _camera->ImageHeight(), + fx, fy, cx, cy, s, + _camera->NearClipPlane(), + _camera->FarClipPlane()); + _camera->SetProjectionMatrix(projectionMatrix); + } + + // Update the DOM object intrinsics to have consistent + // projection matrix values between ogre camera and camera_info msg + // If these values are not defined in the SDF then we need to update + // these values to something reasonable. The projection matrix is + // the cumulative effect of intrinsic and extrinsic parameters + if(!_cameraSdf.HasLensProjection()) + { + // Note that the matrix from Ogre via camera->ProjectionMatrix() has a + // different format than the projection matrix used in SDFormat. + // This is why they are converted using projectionToCameraIntrinsic. + // The resulting matrix is the intrinsic matrix, but since the user has + // not overridden the values, this is also equal to the projection matrix. + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + _camera->ProjectionMatrix(), + _camera->ImageWidth(), + _camera->ImageHeight() + ); + _cameraSdf.SetLensProjectionFx(intrinsicMatrix(0, 0)); + _cameraSdf.SetLensProjectionFy(intrinsicMatrix(1, 1)); + _cameraSdf.SetLensProjectionCx(intrinsicMatrix(0, 2)); + _cameraSdf.SetLensProjectionCy(intrinsicMatrix(1, 2)); + } + // set custom projection matrix based on projection param specified in sdf + else + { + // tx and ty are not used + double fx = _cameraSdf.LensProjectionFx(); + double fy = _cameraSdf.LensProjectionFy(); + double cx = _cameraSdf.LensProjectionCx(); + double cy = _cameraSdf.LensProjectionCy(); + double s = 0; + + auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + _camera->ImageWidth(), + _camera->ImageHeight(), + fx, fy, cx, cy, s, + _camera->NearClipPlane(), + _camera->FarClipPlane()); + _camera->SetProjectionMatrix(projectionMatrix); + } +} + ////////////////////////////////////////////////// math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix( double _imageWidth, double _imageHeight, diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index cbd7f896..31b9b028 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -327,7 +327,7 @@ bool DepthCameraSensor::Load(const sdf::Sensor &_sdf) ////////////////////////////////////////////////// bool DepthCameraSensor::CreateCamera() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { @@ -341,7 +341,6 @@ bool DepthCameraSensor::CreateCamera() double far = cameraSdf->FarClip(); double near = cameraSdf->NearClip(); - this->PopulateInfo(cameraSdf); this->dataPtr->depthCamera = this->Scene()->CreateDepthCamera( this->Name()); @@ -404,6 +403,9 @@ bool DepthCameraSensor::CreateCamera() this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera, + *cameraSdf); + // Create the directory to store frames if (cameraSdf->SaveFrames()) { @@ -412,6 +414,8 @@ bool DepthCameraSensor::CreateCamera() this->dataPtr->saveImage = true; } + this->PopulateInfo(cameraSdf); + this->dataPtr->depthConnection = this->dataPtr->depthCamera->ConnectNewDepthFrame( std::bind(&DepthCameraSensor::OnNewDepthFrame, this, diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index d780feb2..e0d742e4 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -277,7 +277,7 @@ bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) ////////////////////////////////////////////////// bool RgbdCameraSensor::CreateCameras() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { @@ -285,8 +285,6 @@ bool RgbdCameraSensor::CreateCameras() return false; } - this->PopulateInfo(cameraSdf); - int width = cameraSdf->ImageWidth(); int height = cameraSdf->ImageHeight(); @@ -379,6 +377,11 @@ bool RgbdCameraSensor::CreateCameras() this->Scene()->RootVisual()->AddChild(this->dataPtr->depthCamera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->depthCamera, + *cameraSdf); + + this->PopulateInfo(cameraSdf); + this->dataPtr->depthConnection = this->dataPtr->depthCamera->ConnectNewDepthFrame( std::bind(&RgbdCameraSensorPrivate::OnNewDepthFrame, diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index fd820509..6ebadb6b 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -275,7 +275,7 @@ void SegmentationCameraSensor::SetScene( ///////////////////////////////////////////////// bool SegmentationCameraSensor::CreateCamera() { - const auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); + auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); if (!sdfCamera) { gzerr << "Unable to access camera SDF element\n"; @@ -304,9 +304,6 @@ bool SegmentationCameraSensor::CreateCamera() } } - // Camera Info Msg - this->PopulateInfo(sdfCamera); - // Save frames properties if (sdfCamera->SaveFrames()) { @@ -379,6 +376,9 @@ bool SegmentationCameraSensor::CreateCamera() // Add the rendering sensor to handle its render this->AddSensor(this->dataPtr->camera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->camera, + *sdfCamera); + // Add the rgb camera only if we want to generate dataset / save samples if (this->dataPtr->saveSamples) { @@ -396,8 +396,14 @@ bool SegmentationCameraSensor::CreateCamera() this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera); this->AddSensor(this->dataPtr->rgbCamera); this->dataPtr->image = this->dataPtr->rgbCamera->CreateImage(); + + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->rgbCamera, + *sdfCamera); } + // Camera Info Msg + this->PopulateInfo(sdfCamera); + // Connection to receive the segmentation buffer this->dataPtr->newSegmentationConnection = this->dataPtr->camera->ConnectNewSegmentationFrame( diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 90cab5df..717caf6b 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -247,7 +247,7 @@ bool ThermalCameraSensor::Load(const sdf::Sensor &_sdf) ////////////////////////////////////////////////// bool ThermalCameraSensor::CreateCamera() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { @@ -263,8 +263,6 @@ bool ThermalCameraSensor::CreateCamera() double farPlane = cameraSdf->FarClip(); double nearPlane = cameraSdf->NearClip(); - this->PopulateInfo(cameraSdf); - this->dataPtr->thermalCamera = this->Scene()->CreateThermalCamera( this->Name()); this->dataPtr->thermalCamera->SetImageWidth(width); @@ -346,6 +344,11 @@ bool ThermalCameraSensor::CreateCamera() this->Scene()->RootVisual()->AddChild(this->dataPtr->thermalCamera); + this->UpdateLensIntrinsicsAndProjection(this->dataPtr->thermalCamera, + *cameraSdf); + + this->PopulateInfo(cameraSdf); + // Create the directory to store frames if (cameraSdf->SaveFrames()) { diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index e47f40ad..6c529174 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -154,6 +155,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 +521,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..af0469c1 --- /dev/null +++ b/test/sdf/depth_camera_intrinsics.sdf @@ -0,0 +1,74 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + + + 0.1 + 100 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + + + 0.1 + 100 + + + + 866.23 + 866.23 + 500 + 500 + 0 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + + + 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..eb3fd3c3 --- /dev/null +++ b/test/sdf/depth_camera_projection.sdf @@ -0,0 +1,67 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + + + + 866.23 + 866.23 + 500 + 500 + 300 + 200 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + 0 + + + + + + +