From 0b781d5fd5708799cc062bd3fea96cad062f075a Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 17 Jul 2020 12:33:50 +0200 Subject: [PATCH] Added noise to Lidar sensor (#3001) Co-authored-by: Marc Garcia Puig --- CHANGELOG.md | 1 + Docs/ref_sensors.md | 8 ++++++++ .../Carla/Actor/ActorBlueprintFunctionLibrary.cpp | 10 +++++++++- .../Carla/Source/Carla/Sensor/LidarDescription.h | 3 +++ .../Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp | 10 ++++++++-- 5 files changed, 29 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0f7e42918..aed801eb4 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/Docs/ref_sensors.md b/Docs/ref_sensors.md index cf2882896..bfeedcba3 100644 --- a/Docs/ref_sensors.md +++ b/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 float 0.0 Simulation seconds between sensor captures (ticks). + +noise_stddev +float +0.0 +Standard deviation of noise along the vector of each raycast.
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index b9f1962f9..75e4b86fa 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/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( diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h index 10ff9d849..78442d9f5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/LidarDescription.h +++ b/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; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp index fdaf161e4..7e7b73392 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp +++ b/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);