From 08ab86ba271007b0f54f4a26932e194ee1ad236d Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Mon, 29 Jul 2024 09:40:06 -0700
Subject: [PATCH] Skip apply noise / distortion if parameters are 0s (#450)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 src/CameraSensor.cc      | 39 ++++++++++++++++++++++++++++-----------
 src/DepthCameraSensor.cc | 20 ++++++++++++++------
 src/Lidar.cc             | 12 ++++++++++--
 3 files changed, 52 insertions(+), 19 deletions(-)

diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc
index b0f558a7..92a357a9 100644
--- 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
index 52f88d8c..41ed4e52 100644
--- 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
index 5a1e8a03..c67d5dc4 100644
--- 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)
     {