Added custom drop off of lidar points from the API

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

@@ -825,9 +825,31 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition(
   LowerFOV.Id = TEXT("lower_fov");
   LowerFOV.Type = EActorAttributeType::Float;
   LowerFOV.RecommendedValues = { TEXT("-30.0") };
+  // Atmospheric Attenuation Rate.
+  FActorVariation AtmospAttenRate;
+  AtmospAttenRate.Id = TEXT("atmosphere_attenuation_rate");
+  AtmospAttenRate.Type = EActorAttributeType::Float;
+  AtmospAttenRate.RecommendedValues = { TEXT("0.004") };
+  // Dropoff General Rate
+  FActorVariation DropOffGenRate;
+  DropOffGenRate.Id = TEXT("dropoff_general_rate");
+  DropOffGenRate.Type = EActorAttributeType::Float;
+  DropOffGenRate.RecommendedValues = { TEXT("0.45") };
+  // Dropoff intensity limit.
+  FActorVariation DropOffIntensityLimit;
+  DropOffIntensityLimit.Id = TEXT("dropoff_intensity_limit");
+  DropOffIntensityLimit.Type = EActorAttributeType::Float;
+  DropOffIntensityLimit.RecommendedValues = { TEXT("0.8") };
+  // Dropoff at zero intensity.
+  FActorVariation DropOffAtZeroIntensity;
+  DropOffAtZeroIntensity.Id = TEXT("dropoff_zero_intensity");
+  DropOffAtZeroIntensity.Type = EActorAttributeType::Float;
+  DropOffAtZeroIntensity.RecommendedValues = { TEXT("0.4") };
 
   Definition.Variations.Append(
-      {Channels, Range, PointsPerSecond, Frequency, UpperFOV, LowerFOV});
+      {Channels, Range, PointsPerSecond, Frequency, UpperFOV, LowerFOV,
+          AtmospAttenRate, DropOffGenRate, DropOffIntensityLimit,
+          DropOffAtZeroIntensity});
 
   Success = CheckActorDefinition(Definition);
 }
@@ -1432,6 +1454,14 @@ void UActorBlueprintFunctionLibrary::SetLidar(
       RetrieveActorAttributeToFloat("upper_fov", Description.Variations, Lidar.UpperFovLimit);
   Lidar.LowerFovLimit =
       RetrieveActorAttributeToFloat("lower_fov", Description.Variations, Lidar.LowerFovLimit);
+  Lidar.AtmospAttenRate =
+      RetrieveActorAttributeToFloat("atmosphere_attenuation_rate", Description.Variations, Lidar.AtmospAttenRate);
+  Lidar.DropOffGenRate =
+      RetrieveActorAttributeToFloat("dropoff_general_rate", Description.Variations, Lidar.DropOffGenRate);
+  Lidar.DropOffIntensityLimit =
+      RetrieveActorAttributeToFloat("dropoff_intensity_limit", Description.Variations, Lidar.DropOffIntensityLimit);
+  Lidar.DropOffAtZeroIntensity =
+      RetrieveActorAttributeToFloat("dropoff_zero_intensity", Description.Variations, Lidar.DropOffAtZeroIntensity);
 }
 
 void UActorBlueprintFunctionLibrary::SetGnss(

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

@@ -39,6 +39,24 @@ struct CARLA_API FLidarDescription
   UPROPERTY(EditAnywhere)
   float LowerFovLimit = -30.0f;
 
+  /// Attenuation Rate in the atmosphere in m^-1
+  UPROPERTY(EditAnywhere)
+  float AtmospAttenRate = 0.004f;
+
+  /// General drop off rate
+  UPROPERTY(EditAnywhere)
+  float DropOffGenRate = 0.45f;
+
+  /// General drop off rate
+  UPROPERTY(EditAnywhere)
+  float DropOffIntensityLimit = 0.8f;
+
+  /// General drop off rate
+  UPROPERTY(EditAnywhere)
+  float DropOffAtZeroIntensity = 0.4f;
+
+
+
   /// Wether to show debug points of laser hits in simulator.
   UPROPERTY(EditAnywhere)
   bool ShowDebugPoints = false;

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

@@ -5,10 +5,11 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include <PxScene.h>
-
+#include <cmath>
 #include "Carla.h"
 #include "Carla/Sensor/RayCastLidar.h"
 #include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
+#include "carla/geom/Math.h"
 
 #include <compiler/disable-ue4-macros.h>
 #include "carla/geom/Math.h"
@@ -61,6 +62,11 @@ void ARayCastLidar::CreateLasers()
         Description.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
     LaserAngles.Emplace(VerticalAngle);
   }
+
+  // Compute drop off model parameters
+  DropOffBeta = 1.0f - Description.DropOffAtZeroIntensity;
+  DropOffAlpha = Description.DropOffAtZeroIntensity / Description.DropOffIntensityLimit;
+  DropOffGenActive = Description.DropOffGenRate > std::numeric_limits<float>::epsilon();
 }
 
 void ARayCastLidar::Tick(const float DeltaTime)
@@ -97,7 +103,7 @@ void ARayCastLidar::ReadPoints(const float DeltaTime)
   const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime;
   const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
 
-  LidarMeasurement.Reset(ChannelCount,  PointsToScanWithOneLaser);
+  LidarMeasurement.Reset(ChannelCount, PointsToScanWithOneLaser);
 
 
   GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
@@ -125,16 +131,47 @@ void ARayCastLidar::ReadPoints(const float DeltaTime)
   LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
 }
 
-float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FVector &XYZ, const FHitResult& HitInfo) const
+float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const
 {
-  const float Distance = 0.01f * XYZ.Size();
+  const FVector HitPoint = HitInfo.ImpactPoint - LidarBodyLoc;
-  const float AttenAtm = -0.004;
+  const float Distance = 0.01f * HitPoint.Size();
 
-  const float IntEm = 1.0f;
+  const float AttenAtm = Description.AtmospAttenRate;
+  const float AbsAtm = exp(-AttenAtm * Distance);
 
-  const float AbsAtm = exp(AttenAtm * Distance);
+  const FActorRegistry &Registry = GetEpisode().GetActorRegistry();
 
-  const float IntRec = IntEm * AbsAtm;
+  uint8 label = 69;
+
+//  AActor* actor = HitInfo.Actor.Get();
+//  if (actor != nullptr) {
+//    FActorView view = Registry.Find(actor);
+//
+//    if(view.IsValid()){
+//      const FActorInfo* ActorInfo = view.GetActorInfo();
+//
+//      if(ActorInfo != nullptr) {
+//        //TSet<ECityObjectLabel> labels = ActorInfo->SemanticTags;
+//        //if(labels.Num() == 1)
+//        //    label = static_cast<uint8>(*labels.CreateConstIterator());
+//      }
+//      else {
+//        UE_LOG(LogCarla, Warning, TEXT("Info not valid!!!!"));
+//      }
+//    }
+//    else {
+//      UE_LOG(LogCarla, Warning, TEXT("View not valid %p!!!!"), view.GetActor());
+//    }
+//
+//  }
+//  else {
+//    UE_LOG(LogCarla, Warning, TEXT("Actor not found!!!!"));
+//  }
+
+
+
+
+  const float IntRec = AbsAtm;
 
   return IntRec;
 }
@@ -142,7 +179,7 @@ float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FVector
 bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ, float &Intensity) const
 {
 
-  if(RandomEngine->GetUniformFloat() > 0.55f)
+  if(DropOffGenActive && RandomEngine->GetUniformFloat() < Description.DropOffGenRate)
     return false;
 
   const float VerticalAngle = LaserAngles[Channel];
@@ -173,6 +210,7 @@ bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle
     FCollisionResponseParams::DefaultResponseParam
   );
 
+
   if (HitInfo.bBlockingHit)
   {
     if (Description.ShowDebugPoints)
@@ -190,12 +228,15 @@ bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle
     const FVector hp = HitInfo.ImpactPoint;
     XYZ = actorTransf.Inverse().TransformPosition(hp);
 
-    Intensity = ComputeIntensity(LidarBodyLoc, XYZ, HitInfo);
+    Intensity = ComputeIntensity(LidarBodyLoc, HitInfo);
 
-    if(Intensity*0.5 + 0.6 < RandomEngine->GetUniformFloat())
-      return false;
 
-    return true;
+
+    if(Intensity > Description.DropOffIntensityLimit)
+      return true;
+    else
+      return RandomEngine->GetUniformFloat() < DropOffAlpha * Intensity + DropOffBeta;
+
   } else {
     return false;
   }

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

@@ -51,7 +51,7 @@ private:
   bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point, float& Intensity) const;
 
   /// Compute the received intensity of the point
-  float ComputeIntensity(const FVector &LidarBodyLoc, const FVector &XYZ, const FHitResult& HitInfo) const;
+  float ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const;
 
   UPROPERTY(EditAnywhere)
   FLidarDescription Description;
@@ -59,4 +59,15 @@ private:
   TArray<float> LaserAngles;
 
   FLidarMeasurement LidarMeasurement;
+
+  // Enable/Disable general dropoff of lidar points
+  bool DropOffGenActive;
+
+  // Slope for the intensity dropoff of lidar points, it is calculated
+  // throught the dropoff limit and the dropoff at zero intensity
+  // The points is kept with a probality alpha*Intensity + beta where
+  // alpha = (1 - dropoff_zero_intensity) / droppoff_limit
+  // beta = (1 - dropoff_zero_intensity)
+  float DropOffAlpha;
+  float DropOffBeta;
 };