Skip to content

Commit

Permalink
set camera projection based on sdf params
Browse files Browse the repository at this point in the history 
Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@iche033
iche033 committed May 14, 2024
1 parent a685ba4 commit ccfa340
Show file tree
Hide file tree
Showing 3 changed files with 534 additions and 0 deletions.
387 changes: 387 additions & 0 deletions test/integration/depth_camera.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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<gz::sensors::DepthCameraSensor>(
cameraWithoutIntrinsicsTag);
auto *sensor2 = mgr.CreateSensor<gz::sensors::DepthCameraSensor>(
cameraWithIntrinsicsTag);
auto *sensor3 = mgr.CreateSensor<gz::sensors::DepthCameraSensor>(
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<gz::msgs::Image> helper1(imgTopic1);
WaitForMessageTestHelper<gz::msgs::CameraInfo> helper2(infoTopic1);
WaitForMessageTestHelper<gz::msgs::Image> helper3(imgTopic2);
WaitForMessageTestHelper<gz::msgs::CameraInfo> helper4(infoTopic2);
WaitForMessageTestHelper<gz::msgs::Image> helper5(imgTopic3);
WaitForMessageTestHelper<gz::msgs::CameraInfo> 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<void(const gz::msgs::CameraInfo&)> camera1InfoCallback =
[&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg)
{
camera1Info = _msg;
camera1Counter++;
};
std::function<void(const gz::msgs::CameraInfo&)> camera2InfoCallback =
[&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg)
{
camera2Info = _msg;
camera2Counter++;
};
std::function<void(const gz::msgs::CameraInfo&)> 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<std::mutex> 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<gz::sensors::DepthCameraSensor>(
cameraWithoutProjectionsTag);
auto *sensor2 = mgr.CreateSensor<gz::sensors::DepthCameraSensor>(
cameraWithProjectionsTag);
auto *sensor3 = mgr.CreateSensor<gz::sensors::DepthCameraSensor>(
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<gz::msgs::Image> helper1("/camera1/image");
WaitForMessageTestHelper<gz::msgs::CameraInfo> helper2(infoTopic1);
WaitForMessageTestHelper<gz::msgs::Image> helper3("/camera2/image");
WaitForMessageTestHelper<gz::msgs::CameraInfo> helper4(infoTopic2);
WaitForMessageTestHelper<gz::msgs::Image> helper5(imgTopic3);
WaitForMessageTestHelper<gz::msgs::CameraInfo> 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<void(const gz::msgs::CameraInfo&)> camera1InfoCallback =
[&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg)
{
camera1Info = _msg;
camera1Counter++;
};
std::function<void(const gz::msgs::CameraInfo&)> camera2InfoCallback =
[&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg)
{
camera2Info = _msg;
camera2Counter++;
};
std::function<void(const gz::msgs::CameraInfo&)> 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<std::mutex> 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());
Loading

0 comments on commit ccfa340

Please sign in to comment.