From 818b38859d810ce0b0df29d64bd1a089a5a7530a Mon Sep 17 00:00:00 2001
From: Nate Koenig <nkoenig@users.noreply.github.com>
Date: Mon, 8 Feb 2021 08:09:38 -0800
Subject: [PATCH] Apply noise to lidar point cloud (#86)

* Apply noise to lidar point cloud

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Use ray count instead of range count

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Fix range to ray

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
---
 include/ignition/sensors/Lidar.hh |  5 +++++
 src/GpuLidarSensor.cc             | 14 +++++++++-----
 src/Lidar.cc                      | 28 ++++++++++++++++++++--------
 3 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/include/ignition/sensors/Lidar.hh b/include/ignition/sensors/Lidar.hh
index 1144f059..0d352767 100644
--- a/include/ignition/sensors/Lidar.hh
+++ b/include/ignition/sensors/Lidar.hh
@@ -58,6 +58,11 @@ namespace ignition
       /// \return true if the update was successfull
       public: virtual bool Update(const common::Time &_now) override;
 
+      /// \brief Apply noise to the laser buffer, if noise has been
+      /// configured. This should be called before PublishLidarScan if you
+      /// want the scan data to contain noise.
+      public: void ApplyNoise();
+
       /// \brief Publish LaserScan message
       /// \param[in] _now The current time
       /// \return true if the update was successfull
diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc
index 2cbc059a..9a750e03 100644
--- a/src/GpuLidarSensor.cc
+++ b/src/GpuLidarSensor.cc
@@ -29,7 +29,8 @@ using namespace ignition::sensors;
 class ignition::sensors::GpuLidarSensorPrivate
 {
   /// \brief Fill the point cloud packed message
-  public: void FillPointCloudMsg();
+  /// \param[in] _laserBuffer Lidar data buffer.
+  public: void FillPointCloudMsg(const float *_laserBuffer);
 
   /// \brief Rendering camera
   public: ignition::rendering::GpuRaysPtr gpuRays;
@@ -232,6 +233,9 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now)
   /// \todo(anyone) It would be nice to remove this copy.
   this->dataPtr->gpuRays->Copy(this->laserBuffer);
 
+  // Apply noise before publishing the data.
+  this->ApplyNoise();
+
   this->PublishLidarScan(_now);
 
   if (this->dataPtr->pointPub.HasConnections())
@@ -244,7 +248,7 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now)
 
     this->dataPtr->pointMsg.set_is_dense(true);
 
-    this->dataPtr->FillPointCloudMsg();
+    this->dataPtr->FillPointCloudMsg(this->laserBuffer);
 
     {
       this->AddSequence(this->dataPtr->pointMsg.mutable_header());
@@ -289,7 +293,7 @@ ignition::math::Angle GpuLidarSensor::VFOV() const
 }
 
 //////////////////////////////////////////////////
-void GpuLidarSensorPrivate::FillPointCloudMsg()
+void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer)
 {
   IGN_PROFILE("GpuLidarSensorPrivate::FillPointCloudMsg");
   uint32_t width = this->pointMsg.width();
@@ -322,8 +326,8 @@ void GpuLidarSensorPrivate::FillPointCloudMsg()
     {
       // Index of current point, and the depth value at that point
       auto index = j * width * channels + i * channels;
-      float depth = this->gpuRays->Data()[index];
-      float intensity = this->gpuRays->Data()[index + 1];
+      float depth = _laserBuffer[index];
+      float intensity = _laserBuffer[index + 1];
       uint16_t ring = j;
 
       int fieldIndex = 0;
diff --git a/src/Lidar.cc b/src/Lidar.cc
index d74040c0..6c21b938 100644
--- a/src/Lidar.cc
+++ b/src/Lidar.cc
@@ -192,6 +192,26 @@ bool Lidar::Update(const ignition::common::Time &/*_now*/)
   return false;
 }
 
+//////////////////////////////////////////////////
+void Lidar::ApplyNoise()
+{
+  if (this->dataPtr->noises.find(LIDAR_NOISE) != this->dataPtr->noises.end())
+  {
+    for (unsigned int j = 0; j < this->VerticalRayCount(); ++j)
+    {
+      for (unsigned int i = 0; i < this->RayCount(); ++i)
+      {
+        int index = j * this->RayCount() + i;
+        double range = this->laserBuffer[index*3];
+        range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range);
+        range = ignition::math::clamp(range,
+            this->RangeMin(), this->RangeMax());
+        this->laserBuffer[index*3] = range;
+      }
+    }
+  }
+}
+
 //////////////////////////////////////////////////
 bool Lidar::PublishLidarScan(const ignition::common::Time &_now)
 {
@@ -236,14 +256,6 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now)
       int index = j * this->RangeCount() + i;
       double range = this->laserBuffer[index*3];
 
-      if (this->dataPtr->noises.find(LIDAR_NOISE) !=
-          this->dataPtr->noises.end())
-      {
-        range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range);
-        range = ignition::math::clamp(range,
-            this->RangeMin(), this->RangeMax());
-      }
-
       range = ignition::math::isnan(range) ? this->RangeMax() : range;
       this->dataPtr->laserMsg.set_ranges(index, range);
       this->dataPtr->laserMsg.set_intensities(index,