Skip to content

Commit

Permalink
backport test changes from gz-sensors8
Browse files Browse the repository at this point in the history
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
iche033 committed Oct 5, 2023
1 parent afc65d2 commit 1de1e8a
Showing 1 changed file with 31 additions and 16 deletions.
47 changes: 31 additions & 16 deletions test/integration/camera.cc
Original file line number Diff line number Diff line change
Expand Up @@ -243,6 +243,7 @@ void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine)
box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5));
box->SetLocalRotation(0, 0, 0);
box->SetMaterial(blue);
scene->DestroyMaterial(blue);
root->AddChild(box);

// Do the test
Expand Down Expand Up @@ -465,7 +466,14 @@ void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine)
delete [] img2;
delete [] img3;

// Clean up rendering ptrs
box.reset();
blue.reset();

// Clean up
mgr.Remove(sensor1->Id());
mgr.Remove(sensor2->Id());
mgr.Remove(sensor3->Id());
engine->DestroyScene(scene);
gz::rendering::unloadEngine(engine->Name());
}
Expand All @@ -477,7 +485,6 @@ TEST_P(CameraSensorTest, CameraIntrinsics)
CameraIntrinsics(GetParam());
}


//////////////////////////////////////////////////
void CameraSensorTest::CameraProjection(const std::string &_renderEngine)
{
Expand Down Expand Up @@ -533,6 +540,7 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine)
box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5));
box->SetLocalRotation(0, 0, 0);
box->SetMaterial(blue);
scene->DestroyMaterial(blue);
root->AddChild(box);

// Do the test
Expand Down Expand Up @@ -663,35 +671,35 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine)
// Camera sensor without projection tag
// account for error converting gl projection values back to
// cv projection values
double error = 1;
double error = 1.0;
EXPECT_EQ(camera1Info.width(), width);
EXPECT_EQ(camera1Info.height(), width);
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);
EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500);
EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0);
EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0);
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(), height);
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);
EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500);
EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300);
EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200);
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);
EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900);
EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501);
EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501);
EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0);
EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0);
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);

unsigned int r1Sum = 0u;
unsigned int g1Sum = 0u;
Expand Down Expand Up @@ -757,7 +765,14 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine)
delete [] img2;
delete [] img3;

// Clean up rendering ptrs
box.reset();
blue.reset();

// Clean up
mgr.Remove(sensor1->Id());
mgr.Remove(sensor2->Id());
mgr.Remove(sensor3->Id());
engine->DestroyScene(scene);
gz::rendering::unloadEngine(engine->Name());
}
Expand Down

0 comments on commit 1de1e8a

Please sign in to comment.