Skip to content
This repository was archived by the owner on Feb 3, 2025. It is now read-only.

Commit

Permalink
Fix wide-angle lens flare occlusion lag (#3325)
Browse files Browse the repository at this point in the history 
* Force update of wide angle dummy camera

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

* Add test using cameras with slow update rate

The test adds a model with wide-angle cameras updated at 1 Hz
that demonstrate the bug reported in #3310 when moved
back and forth. The lag effect is tested by loading with
--lockstep and the cameras initially occluded, moving
the cameras to an unoccluded pose while paused, and then
confirming that the next image correctly has lens flare.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@audrow @scpeters
audrow and scpeters authored May 25, 2023
1 parent 00c11fd commit 7f49c14
Show file tree
Hide file tree
Showing 3 changed files with 229 additions and 2 deletions.
2 changes: 2 additions & 0 deletions gazebo/rendering/LensFlare.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,6 +305,8 @@ namespace gazebo
ignition::math::Pose3d(
Conversions::ConvertIgn(cam->getDerivedPosition()),
Conversions::ConvertIgn(quat)));
this->dataPtr->wideAngleDummyCamera->OgreCamera()
->getParentSceneNode()->_update(true, true);

// OcclusionScale() was built for a regular perspective projection
// camera and cannot be passed a WideAngleCamera. A cleaner solution
Expand Down
170 changes: 168 additions & 2 deletions test/integration/camera_sensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1433,8 +1433,11 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
physics::ModelPtr model = world->ModelByName(modelNameLensFlare);
ASSERT_TRUE(model != nullptr);

// use fully scoped sensor name
sensors::SensorPtr sensorLensFlare =
sensors::get_sensor(cameraNameLensFlare);
sensors::get_sensor(world->Name() + "::"
+ modelNameLensFlare + "::link::"
+ cameraNameLensFlare);
sensors::CameraSensorPtr camSensorLensFlare =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensorLensFlare);
ASSERT_TRUE(camSensorLensFlare != nullptr);
Expand All @@ -1446,8 +1449,11 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
model = world->ModelByName(modelNameWithoutLensFlare);
ASSERT_TRUE(model != nullptr);

// use fully scoped sensor name
sensors::SensorPtr sensorWithoutLensFlare =
sensors::get_sensor(cameraNameWithoutLensFlare);
sensors::get_sensor(world->Name() + "::"
+ modelNameWithoutLensFlare + "::link::"
+ cameraNameWithoutLensFlare);
sensors::CameraSensorPtr camSensorWithoutLensFlare =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensorWithoutLensFlare);
ASSERT_TRUE(camSensorWithoutLensFlare != nullptr);
Expand Down Expand Up @@ -1512,6 +1518,166 @@ TEST_F(CameraSensor, LensFlareWideAngleCamera)
delete[] img2;
}

/////////////////////////////////////////////////
TEST_F(CameraSensor, LensFlareWideAngleCameraOcclusion)
{
LoadArgs(" --lockstep -u worlds/lensflare_wideangle_cam.world");

// Make sure the render engine is available.
if (rendering::RenderEngine::Instance()->GetRenderPathType() ==
rendering::RenderEngine::NONE)
{
gzerr << "No rendering engine, unable to run camera test\n";
return;
}

// Get the model containing the wide angle cameras.
const std::string modelName = "slow_wide_angle_cameras";
const std::string cameraNameLensFlare = "camera_sensor_lensflare";

physics::WorldPtr world = physics::get_world();
ASSERT_TRUE(world != nullptr);
physics::ModelPtr model = world->ModelByName(modelName);
ASSERT_TRUE(model != nullptr);

// use fully scoped sensor name
sensors::SensorPtr sensorLensFlare =
sensors::get_sensor(world->Name() + "::"
+ modelName + "::link::" + cameraNameLensFlare);
sensors::CameraSensorPtr camSensorLensFlare =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensorLensFlare);
ASSERT_TRUE(camSensorLensFlare != nullptr);

// Get the wide angle camera without the lens flare.
std::string cameraNameWithoutLensFlare = "camera_sensor_without_lensflare";

// use fully scoped sensor name
sensors::SensorPtr sensorWithoutLensFlare =
sensors::get_sensor(world->Name() + "::"
+ modelName + "::link::" + cameraNameWithoutLensFlare);
sensors::CameraSensorPtr camSensorWithoutLensFlare =
std::dynamic_pointer_cast<sensors::CameraSensor>(sensorWithoutLensFlare);
ASSERT_TRUE(camSensorWithoutLensFlare != nullptr);

const unsigned int width = 320;
const unsigned int height = 240;

// Collect images from the 2 cameras.
imageCount = 0;
imageCount2 = 0;
img = new unsigned char[width * height * 3];
img2 = new unsigned char[width * height * 3];
event::ConnectionPtr c =
camSensorLensFlare->Camera()->ConnectNewImageFrame(
std::bind(&::OnNewCameraFrame, &imageCount, img,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
event::ConnectionPtr c2 =
camSensorWithoutLensFlare->Camera()->ConnectNewImageFrame(
std::bind(&::OnNewCameraFrame, &imageCount2, img2,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));

// Step just enough to trigger new images, then wait for images to arrive.
for (int i = 0; i < 3; ++i)
{
// Sensor update period of 1 second
// Step 1.005 seconds to ensure images are triggered
world->Step(1005);

int sleep = 0;
while ((imageCount < i+1 || imageCount2 < i+1)
&& sleep++ < 1000)
{
common::Time::MSleep(10);
}
gzdbg << "Sim time " << world->SimTime().Double()
<< ", slept for " << sleep * 0.01 << " seconds"
<< std::endl;

EXPECT_EQ(imageCount, i + 1);
EXPECT_EQ(imageCount2, i + 1);
}

EXPECT_EQ(imageCount, 3);
EXPECT_EQ(imageCount2, 3);

// Lock image mutex to allow safe comparison of images
{
std::lock_guard<std::mutex> lock(mutex);
// The lens flare should be occluded at the initial position, so expect
// the images with and without lens flare to be identical.
for (unsigned int y = 0; y < height; ++y)
{
for (unsigned int x = 0; x < width*3; x+=3)
{
EXPECT_NEAR(img[(y*width*3) + x], img2[(y*width*3) + x], 2);
EXPECT_NEAR(img[(y*width*3) + x + 1], img2[(y*width*3) + x + 1], 2);
EXPECT_NEAR(img[(y*width*3) + x + 2], img2[(y*width*3) + x + 2], 2);
}
}
}

// Move the model up 10 meters so that it is no longer occluded.
auto modelPose = model->WorldPose();
modelPose.SetZ(modelPose.Pos().Z() + 10);
model->SetWorldPose(modelPose);

// Step an additional 1.005 seconds.
// This should generate another pair of images.
world->Step(1005);
{
int sleep = 0;
while ((imageCount < 4 || imageCount2 < 4)
&& sleep++ < 1000)
{
common::Time::MSleep(10);
}
gzdbg << "Sim time " << world->SimTime().Double()
<< ", slept for " << sleep * 0.01 << " seconds"
<< std::endl;
}
EXPECT_EQ(imageCount, 4);
EXPECT_EQ(imageCount2, 4);

c.reset();
c2.reset();

unsigned int colorSum = 0;
unsigned int colorSum2 = 0;
// Lock image mutex to allow safe comparison of images
{
std::lock_guard<std::mutex> lock(mutex);
EXPECT_EQ(imageCount, 4);
EXPECT_EQ(imageCount2, 4);

for (unsigned int y = 0; y < height; ++y)
{
for (unsigned int x = 0; x < width*3; x+=3)
{
unsigned int r = img[(y*width*3) + x];
unsigned int g = img[(y*width*3) + x + 1];
unsigned int b = img[(y*width*3) + x + 2];
colorSum += r + g + b;
unsigned int r2 = img2[(y*width*3) + x];
unsigned int g2 = img2[(y*width*3) + x + 1];
unsigned int b2 = img2[(y*width*3) + x + 2];
colorSum2 += r2 + g2 + b2;
}
}
}

// Camera with lens flare should be brighter than camera without it.
EXPECT_GT(colorSum, 0u) << "colorSum: " << colorSum;
EXPECT_GT(colorSum2, 0u) << "colorSum2: " << colorSum2;
EXPECT_GT(colorSum, colorSum2 + 10000000) <<
"colorSum: " << colorSum << ", " <<
"colorSum2: " << colorSum2;

delete[] img;
delete[] img2;
}

/////////////////////////////////////////////////
TEST_F(CameraSensor, 16bit)
{
Expand Down
59 changes: 59 additions & 0 deletions worlds/lensflare_wideangle_cam.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,5 +385,64 @@
</link>
</model>

<!-- model with two wide-angle cameras with slow update rates, with and without lens flare plugin -->
<model name="slow_wide_angle_cameras">
<static>true</static>
<pose>0 12 11.0 0 0 0</pose>
<link name="link">
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="camera_sensor_lensflare" type="wideanglecamera">
<camera>
<horizontal_fov>3.14159</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<lens>
<type>equidistant</type>
<scale_to_hfov>true</scale_to_hfov>
<cutoff_angle>1.5707</cutoff_angle>
<env_texture_size>512</env_texture_size>
</lens>
</camera>
<always_on>1</always_on>
<update_rate>1</update_rate>
<visualize>true</visualize>
<plugin name="lensflare" filename="libLensFlareSensorPlugin.so"/>
</sensor>
<sensor name="camera_sensor_without_lensflare" type="wideanglecamera">
<camera>
<horizontal_fov>3.14159</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<lens>
<type>equidistant</type>
<scale_to_hfov>true</scale_to_hfov>
<cutoff_angle>1.5707</cutoff_angle>
<env_texture_size>512</env_texture_size>
</lens>
</camera>
<always_on>1</always_on>
<update_rate>1</update_rate>
<visualize>true</visualize>
</sensor>
</link>
</model>
</world>
</sdf>

0 comments on commit 7f49c14

Please sign in to comment.