fix(thermal_camera): Fixed Gaussian noise for thermal camera. (#597)

Signed-off-by: Martin Pecka <peci1@seznam.cz>
(cherry picked from commit 0e561e3)

Author: peci1

src/ThermalCameraSensor.cc

@@ -40,6 +40,7 @@
 
 #include "gz/sensors/ThermalCameraSensor.hh"
 #include "gz/sensors/ImageGaussianNoiseModel.hh"
+#include "gz/sensors/ImageNoise.hh"
 #include "gz/sensors/RenderingEvents.hh"
 #include "gz/sensors/SensorFactory.hh"
 
@@ -317,12 +318,20 @@ bool ThermalCameraSensor::CreateCamera()
     // Add gaussian noise to camera sensor
     if (noiseSdf.Type() == sdf::NoiseType::GAUSSIAN)
     {
-      this->dataPtr->noises[noiseType] =
-        NoiseFactory::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, "thermal_camera");
 
-      std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
-           this->dataPtr->noises[noiseType])->SetCamera(
-             this->dataPtr->thermalCamera);
+        std::dynamic_pointer_cast<ImageGaussianNoiseModel>(
+             this->dataPtr->noises[noiseType])->SetCamera(
+               this->dataPtr->thermalCamera);
+      }
     }
     else if (noiseSdf.Type() != sdf::NoiseType::NONE)
     {

test/integration/thermal_camera.cc

@@ -126,6 +126,12 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF(
   auto imagePtr = cameraPtr->GetElement("image");
   ASSERT_TRUE(cameraPtr->HasElement("clip"));
   auto clipPtr = cameraPtr->GetElement("clip");
+  ASSERT_TRUE(cameraPtr->HasElement("noise"));
+  auto noisePtr = cameraPtr->GetElement("noise");
+
+  EXPECT_EQ("gaussian", noisePtr->Get<std::string>("type"));
+  EXPECT_FLOAT_EQ(0.0, noisePtr->Get<double>("mean"));
+  EXPECT_DOUBLE_EQ(0.007, noisePtr->Get<double>("stddev"));
 
   int imgWidth = imagePtr->Get<int>("width");
   int imgHeight = imagePtr->Get<int>("height");
@@ -386,6 +392,12 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF(
   auto cameraPtr = sensorPtr->GetElement("camera");
   ASSERT_TRUE(cameraPtr->HasElement("image"));
   auto imagePtr = cameraPtr->GetElement("image");
+  ASSERT_TRUE(cameraPtr->HasElement("noise"));
+  auto noisePtr = cameraPtr->GetElement("noise");
+
+  EXPECT_EQ("gaussian", noisePtr->Get<std::string>("type"));
+  EXPECT_FLOAT_EQ(0.0, noisePtr->Get<double>("mean"));
+  EXPECT_DOUBLE_EQ(0.007, noisePtr->Get<double>("stddev"));
 
   std::string format = imagePtr->Get<std::string>("format");
   EXPECT_EQ("L8", format);

test/sdf/thermal_camera_sensor_8bit_builtin.sdf

@@ -16,6 +16,11 @@
             <near>0.1</near>
             <far>10.0</far>
           </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
         </camera>
       </sensor>
     </link>

test/sdf/thermal_camera_sensor_builtin.sdf

@@ -16,6 +16,11 @@
             <near>0.1</near>
             <far>10.0</far>
           </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
         </camera>
       </sensor>
     </link>