Added noise to Lidar sensor (#3001)

Co-authored-by: Marc Garcia Puig <marcgpuig@gmail.com>

CHANGELOG.md

@@ -35,6 +35,7 @@
   * Fixed collision issues when debug draw(debug.draw_line) is called
   * Fixed Gyroscope sensor to properly give angular velocity readings in local frame
   * Added Renderdoc plugin to the Unreal project
+  * Added configurable noise to Lidar sensor
   * Replace deprectated `platform.dist()` with recommended `distro.linux_distribution()`
 
 ## CARLA 0.9.9

Docs/ref_sensors.md

@@ -472,6 +472,9 @@ This sensor simulates a rotating Lidar implemented using ray-casting.
 The points are computed by adding a laser for each channel distributed in the vertical FOV. The rotation is simulated computing the horizontal angle that the Lidar rotated in a frame. The point cloud is calculated by doing a ray-cast for each laser in every step:
 `points_per_channel_each_step = points_per_second / (FPS * channels)`
 
+If the `noise_stddev` attribute is positive, each point is then randomly perturbed along the vector of the laser ray. In effect, this simulates
+a Lidar with perfect angular positioning, but noisy distance measurement.
+
 A Lidar measurement contains a packet with all the points generated during a `1/FPS` interval. During this interval the physics are not updated so all the points in a measurement reflect the same "static picture" of the scene.
 
 The information of the Lidar measurement is enconded 4D points. Being the first three, the space points in xyz coordinates and the last one intensity loss during the travel. This intensity is computed by:
@@ -558,6 +561,11 @@ The rotation of the LIDAR can be tuned to cover a specific angle on every simula
 <td>float</td>
 <td>0.0</td>
 <td>Simulation seconds between sensor captures (ticks).</td>
+<tr>
+<td><code>noise_stddev</code></td>
+<td>float</td>
+<td>0.0</td>
+<td>Standard deviation of noise along the vector of each raycast.</td>
 </tbody>
 </table>
 <br>

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp

@@ -846,10 +846,16 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition(
   DropOffAtZeroIntensity.Type = EActorAttributeType::Float;
   DropOffAtZeroIntensity.RecommendedValues = { TEXT("0.4") };
 
+  // Lower FOV limit.
+  FActorVariation StdDevLidar;
+  StdDevLidar.Id = TEXT("noise_stddev");
+  StdDevLidar.Type = EActorAttributeType::Float;
+  StdDevLidar.RecommendedValues = { TEXT("0.0") };
+
   Definition.Variations.Append(
       {Channels, Range, PointsPerSecond, Frequency, UpperFOV, LowerFOV,
           AtmospAttenRate, DropOffGenRate, DropOffIntensityLimit,
-          DropOffAtZeroIntensity});
+          DropOffAtZeroIntensity, StdDevLidar});
 
   Success = CheckActorDefinition(Definition);
 }
@@ -1462,6 +1468,8 @@ void UActorBlueprintFunctionLibrary::SetLidar(
       RetrieveActorAttributeToFloat("dropoff_intensity_limit", Description.Variations, Lidar.DropOffIntensityLimit);
   Lidar.DropOffAtZeroIntensity =
       RetrieveActorAttributeToFloat("dropoff_zero_intensity", Description.Variations, Lidar.DropOffAtZeroIntensity);
+  Lidar.NoiseStdDev =
+      RetrieveActorAttributeToFloat("noise_stddev", Description.Variations, Lidar.NoiseStdDev);
 }
 
 void UActorBlueprintFunctionLibrary::SetGnss(

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h

@@ -58,4 +58,7 @@ struct CARLA_API FLidarDescription
   /// Wether to show debug points of laser hits in simulator.
   UPROPERTY(EditAnywhere)
   bool ShowDebugPoints = false;
+
+  UPROPERTY(EditAnywhere)
+  float NoiseStdDev = 0.0f;
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp

@@ -167,7 +167,8 @@ bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle
     LidarBodyRot
   );
   const auto Range = Description.Range;
-  FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
+  const FVector ForwardVector = UKismetMathLibrary::GetForwardVector(ResultRot);
+  FVector EndTrace = Range * ForwardVector + LidarBodyLoc;
 
   GetWorld()->LineTraceSingleByChannel(
     HitInfo,
@@ -192,7 +193,12 @@ bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle
       );
     }
 
-    const FVector hp = HitInfo.ImpactPoint;
+    FVector hp = HitInfo.ImpactPoint;
+    if (Description.NoiseStdDev > 0.0f)
+    {
+      const FVector Noise = ForwardVector * RandomEngine->GetNormalDistribution(0.0f, Description.NoiseStdDev);
+      hp += Noise;
+    }
     XYZ = actorTransf.Inverse().TransformPosition(hp);
 
     Intensity = ComputeIntensity(LidarBodyLoc, HitInfo);