Skip apply noise / distortion if parameters are 0s (#450)

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Jul 29, 2024 · 08ab86b · 08ab86b
1 parent f8a1a2e
commit 08ab86b
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 19 deletions.
diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc
@@ -224,12 +224,20 @@ bool CameraSensor::CreateCamera()
     // Add gaussian noise to camera sensor
     if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
     {
-      this->dataPtr->noises[noiseType] =
-        ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera");
+      // Skip applying noise if mean and stddev are 0 - this avoids
+      // doing an extra render pass in gz-rendering
+      // Note ImageGaussianNoiseModel only uses mean and stddev and does not
+      // use bias parameters.
+      if (!math::equal(noiseSdf.Mean(), 0.0) ||
+          !math::equal(noiseSdf.StdDev(), 0.0))
+      {
+        this->dataPtr->noises[noiseType] =
+          ImageNoiseFactory::NewNoiseModel(noiseSdf, "camera");
 
-      std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
-           this->dataPtr->noises[noiseType])->SetCamera(
-             this->dataPtr->camera);
+        std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
+             this->dataPtr->noises[noiseType])->SetCamera(
+               this->dataPtr->camera);
+      }
     }
     else if (noiseSdf.Type() != sdf::NoiseType::NONE)
     {
@@ -253,13 +261,22 @@ bool CameraSensor::CreateCamera()
   this->dataPtr->camera->SetHFOV(angle);
 
   if (cameraSdf->Element() != nullptr &&
-      cameraSdf->Element()->HasElement("distortion")) {
-    this->dataPtr->distortion =
-        ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera");
-    this->dataPtr->distortion->Load(*cameraSdf);
+      cameraSdf->Element()->HasElement("distortion"))
+  {
+    // Skip distortion of all coefficients are 0s
+    if (!math::equal(cameraSdf->DistortionK1(), 0.0) ||
+        !math::equal(cameraSdf->DistortionK2(), 0.0) ||
+        !math::equal(cameraSdf->DistortionK3(), 0.0) ||
+        !math::equal(cameraSdf->DistortionP1(), 0.0) ||
+        !math::equal(cameraSdf->DistortionP2(), 0.0))
+    {
+      this->dataPtr->distortion =
+          ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera");
+      this->dataPtr->distortion->Load(*cameraSdf);
 
-    std::dynamic_pointer_cast<ImageBrownDistortionModel>(
-        this->dataPtr->distortion)->SetCamera(this->dataPtr->camera);
+      std::dynamic_pointer_cast<ImageBrownDistortionModel>(
+          this->dataPtr->distortion)->SetCamera(this->dataPtr->camera);
+    }
   }
 
   sdf::PixelFormatType pixelFormat = cameraSdf->PixelFormat();

diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc
@@ -362,12 +362,20 @@ bool DepthCameraSensor::CreateCamera()
     // Add gaussian noise to camera sensor
     if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
     {
-      this->dataPtr->noises[noiseType] =
-        ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth");
-
-      std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
-           this->dataPtr->noises[noiseType])->SetCamera(
-             this->dataPtr->depthCamera);
+      // Skip applying noise if mean and stddev are 0 - this avoids
+      // doing an extra render pass in gz-rendering
+      // Note ImageGaussianNoiseModel only uses mean and stddev and does not
+      // use bias parameters.
+      if (!math::equal(noiseSdf.Mean(), 0.0) ||
+          !math::equal(noiseSdf.StdDev(), 0.0))
+      {
+        this->dataPtr->noises[noiseType] =
+          ImageNoiseFactory::NewNoiseModel(noiseSdf, "depth");
+
+        std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
+             this->dataPtr->noises[noiseType])->SetCamera(
+               this->dataPtr->depthCamera);
+      }
     }
     else if (noiseSdf.Type() != sdf::NoiseType::NONE)
     {

diff --git a/src/Lidar.cc b/src/Lidar.cc
@@ -172,8 +172,16 @@ bool Lidar::Load(const sdf::Sensor &_sdf)
   {
     if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
     {
-      this->dataPtr->noises[noiseType] =
-        NoiseFactory::NewNoiseModel(noiseSdf);
+      // Skip applying noise if gaussian noise params are all 0s
+      if (!math::equal(noiseSdf.Mean(), 0.0) ||
+          !math::equal(noiseSdf.StdDev(), 0.0) ||
+          !math::equal(noiseSdf.BiasMean(), 0.0) ||
+          !math::equal(noiseSdf.DynamicBiasStdDev(), 0.0) ||
+          !math::equal(noiseSdf.DynamicBiasCorrelationTime(), 0.0))
+      {
+        this->dataPtr->noises[noiseType] =
+          NoiseFactory::NewNoiseModel(noiseSdf);
+      }
     }
     else if (noiseSdf.Type() != sdf::NoiseType::NONE)
     {