Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update Camera Intrinsics in camera_info topic #281

Merged
merged 2 commits into from
Nov 21, 2022
Merged

Update Camera Intrinsics in camera_info topic #281

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b65ff2f
Ported tests and code
adityapande-1995 Nov 2, 2022
116f69e
Merge branch 'gz-sensors7' into aditya/refactor_PR_265
adityapande-1995 Nov 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 23 additions & 3 deletions src/CameraSensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
#include "gz/sensors/SensorFactory.hh"
#include "gz/sensors/SensorTypes.hh"

#include "gz/rendering/Utils.hh"

using namespace gz;
using namespace sensors;

Expand Down Expand Up @@ -141,15 +143,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();

Expand Down Expand Up @@ -237,6 +237,26 @@ 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())
{
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));
}

// Populate camera info topic
this->PopulateInfo(cameraSdf);

return true;
}

Expand Down
131 changes: 131 additions & 0 deletions test/integration/camera.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,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)
Expand Down Expand Up @@ -170,6 +173,134 @@ TEST_P(CameraSensorTest, ImagesWithBuiltinSDF)
ImagesWithBuiltinSDF(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<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 infoTopic1 = "/camera1/camera_info";
std::string infoTopic2 = "/camera2/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);
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<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++;
};

// 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<std::mutex> 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());
}

//////////////////////////////////////////////////
void CameraSensorTest::ImageFormatLInt8LInt16(const std::string &_renderEngine)
{
Expand Down
42 changes: 42 additions & 0 deletions test/sdf/camera_intrinsics.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>