WIP! Refactoring distribution of the rays.

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

@@ -24,6 +24,9 @@ ARadar::ARadar(const FObjectInitializer& ObjectInitializer)
   TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
   TraceParams.bTraceComplex = true;
   TraceParams.bReturnPhysicalMaterial = false;
+
+  // TODO: default params
+
 }
 
 void ARadar::Set(const FActorDescription &ActorDescription)
@@ -32,29 +35,25 @@ void ARadar::Set(const FActorDescription &ActorDescription)
   UActorBlueprintFunctionLibrary::SetRadar(ActorDescription, this);
 }
 
-void ARadar::SetFOVAndSteps(float NewFov, int NewSteps)
+void ARadar::SetHorizontalFOV(float NewHorizontalFOV)
 {
-  FOV = NewFov;
+  HorizontalFOV = NewHorizontalFOV;
-  Steps = NewSteps;
+}
-  PreCalculateCosSin();
+
-  PreCalculateLineTraceIncrement();
+void  ARadar::SetVerticalFOV(float NewVerticalFOV)
+{
+  VerticalFOV = NewVerticalFOV;
 }
 
 void ARadar::SetDistance(float NewDistance)
 {
   Distance = NewDistance;
-  PreCalculateLineTraceIncrement();
 }
 
-void ARadar::SetAperture(int NewAperture)
+void ARadar::SetPointsPerSecond(float NewPointsPerSecond)
 {
-  Aperture = NewAperture;
+  PointsPerSecond = NewPointsPerSecond;
-  PreCalculateLineTraceIncrement();
+  RadarData.SetResolution(PointsPerSecond);
-}
-
-void ARadar::SetPointLossPercentage(float NewLossPercentage)
-{
-  PointLossPercentage = NewLossPercentage;
 }
 
 void ARadar::BeginPlay()
@@ -64,20 +63,16 @@ void ARadar::BeginPlay()
   World = GetWorld();
 
   PrevLocation = GetActorLocation();
-
-  PreCalculateCosSin();
 }
 
 void ARadar::Tick(const float DeltaTime)
 {
   Super::Tick(DeltaTime);
 
-  namespace css = carla::sensor::s11n;
-
   CalculateCurrentVelocity(DeltaTime);
 
   RadarData.Reset();
-  SendLineTraces(DeltaTime);
+  SendLineTraces();
 
   auto DataStream = GetDataStream(*this);
   DataStream.Send(*this, RadarData, DataStream.PopBufferFromPool());
@@ -90,37 +85,35 @@ void ARadar::CalculateCurrentVelocity(const float DeltaTime)
   PrevLocation = RadarLocation;
 }
 
-void ARadar::PreCalculateCosSin()
+void ARadar::SendLineTraces()
 {
-  float AngleIncrement = FMath::DegreesToRadians(360.0f / Steps);
-  FMath::SinCos(&CosSinIncrement.Y, &CosSinIncrement.X, AngleIncrement);
-}
 
-void ARadar::PreCalculateLineTraceIncrement()
-{
-  // Compute and set the total amount of rays
-  uint32 TotalRayNumber = (Steps * Aperture) + 1u;
-  RadarData.SetResolution(TotalRayNumber);
-  LineTraceIncrement = FMath::Tan(
-      FMath::DegreesToRadians(FOV * 0.5f)) * Distance / (Aperture + 1);
-}
-
-void ARadar::SendLineTraces(float DeltaSeconds)
-{
   constexpr float TO_METERS = 1e-2;
   FHitResult OutHit(ForceInit);
-
-  const FVector& RadarLocation = GetActorLocation();
-  const FVector& ForwardVector = GetActorForwardVector();
   const FTransform& ActorTransform = GetActorTransform();
   const FRotator& TransformRotator = ActorTransform.Rotator();
+  const FVector& RadarLocation = GetActorLocation();
+  const FVector& ForwardVector = GetActorForwardVector();
   const FVector TransformXAxis = ActorTransform.GetUnitAxis(EAxis::X);
   const FVector TransformYAxis = ActorTransform.GetUnitAxis(EAxis::Y);
   const FVector TransformZAxis = ActorTransform.GetUnitAxis(EAxis::Z);
 
-  const FVector WorldForwardVector = ForwardVector * Distance;
+  // Maximun radar radius in horizontal and vertical direction
-  FVector EndLocation = RadarLocation + WorldForwardVector;
+  const float MaxRx = FMath::Tan(FMath::DegreesToRadians(HorizontalFOV * 0.5f)) * Distance;
-  FVector2D CurrentCosSin = {1.0f, 0.0f};
+  const float MaxRy = FMath::Tan(FMath::DegreesToRadians(VerticalFOV * 0.5f)) * Distance;
+
+  for (int i = 0; i < NumPoints; i++)
+  {
+    float Radius = RandomEngine->GetUniformFloat();
+    float Angle = RandomEngine->GetUniformFloatInRange(0.0f, 2.0f * Math.PI);
+    float Sin, Cos;
+    FMath::SinCos(&Sin, &Cos, Angle);
+    FVector Rotation = TransformRotator.RotateVector({
+      0.0f,
+      MaxRx * Radius * Cos,
+      MaxRy * Radius * Sin
+    });
+    FVector EndLocation = Rotation * Distance;
 
     bool Hitted = World->LineTraceSingleByChannel(
       OutHit,
@@ -131,40 +124,6 @@ void ARadar::SendLineTraces(float DeltaSeconds)
       FCollisionResponseParams::DefaultResponseParam
     );
 
-  if (Hitted)
-  {
-    const float RelativeVelocity = CalculateRelativeVelocity(OutHit, RadarLocation, ForwardVector);
-    RadarData.WriteDetection({RelativeVelocity, 0.0f, 0.0f, OutHit.Distance * TO_METERS});
-  }
-
-  for(int j = 0; j < Steps; j++)
-  {
-    EndLocation = RadarLocation + WorldForwardVector;
-
-    for(int i = 1; i <= Aperture; i++)
-    {
-      FVector Rotation = TransformRotator.RotateVector({0.0f, CurrentCosSin.X, CurrentCosSin.Y});
-
-      EndLocation += Rotation * LineTraceIncrement;
-
-      // PointLossPercentage is in range [0.0 - 1.0]
-      // e.g: If PointLossPercentage is 0.7, the 70% of the rays are lost
-      // TODO: Improve the performance precalculating the noise probabilty offline
-      if (RandomEngine->GetBoolWithWeight(PointLossPercentage))
-      {
-        // Do not compute the current ray
-        continue;
-      }
-
-      Hitted = World->LineTraceSingleByChannel(
-          OutHit,
-          RadarLocation,
-          EndLocation,
-          ECC_MAX,
-          TraceParams,
-          FCollisionResponseParams::DefaultResponseParam
-      );
-
     TWeakObjectPtr<AActor> HittedActor = OutHit.Actor;
     if (Hitted && HittedActor.Get()) {
 
@@ -186,12 +145,6 @@ void ARadar::SendLineTraces(float DeltaSeconds)
     }
   }
 
-    float NewCos = CosSinIncrement.X * CurrentCosSin.X - CosSinIncrement.Y * CurrentCosSin.Y;
-    float NewSin = CosSinIncrement.Y * CurrentCosSin.X + CosSinIncrement.X * CurrentCosSin.Y;
-    CurrentCosSin.X = NewCos;
-    CurrentCosSin.Y = NewSin;
-  }
-
 }
 
 float ARadar::CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation, const FVector& ForwardVector)

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

@@ -33,16 +33,16 @@ public:
   void Set(const FActorDescription &Description) override;
 
   UFUNCTION(BlueprintCallable, Category = "Radar")
-  void SetFOVAndSteps(float NewFov, int NewSteps);
+  void SetHorizontalFOV(float NewHorizontalFOV);
+
+  UFUNCTION(BlueprintCallable, Category = "Radar")
+  void SetVerticalFOV(float NewVerticalFOV);
 
   UFUNCTION(BlueprintCallable, Category = "Radar")
   void SetDistance(float NewDistance);
 
   UFUNCTION(BlueprintCallable, Category = "Radar")
-  void SetAperture(int NewAperture);
+  void SetPointsPerSecond(int NewPointsPerSecond);
-
-  UFUNCTION(BlueprintCallable, Category = "Radar")
-  void SetPointLossPercentage(float NewLossPercentage);
 
 protected:
 
@@ -54,27 +54,19 @@ protected:
   float Distance;
 
   UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
-  float FOV;
+  float HorizontalFOV;
-
-  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="Detection")
-  int Aperture;
 
   UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
-  int Steps;
+  float VerticalFOV;
 
-  /// Noise threshold [0.0, 1.0] of rays that will be
+  UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
-  UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Detection")
+  int PointsPerSecond;
-  float PointLossPercentage;
 
 private:
 
   void CalculateCurrentVelocity(const float DeltaTime);
 
-  void PreCalculateCosSin();
+  void SendLineTraces();
-
-  void PreCalculateLineTraceIncrement();
-
-  void SendLineTraces(float DeltaSeconds);
 
   float CalculateRelativeVelocity(const FHitResult& OutHit, const FVector& RadarLocation, const FVector& ForwardVector);
 
@@ -82,18 +74,12 @@ private:
 
   FCollisionQueryParams TraceParams;
 
-  // Current Radar Velocity
+  /// Current Radar Velocity
   FVector CurrentVelocity;
 
   /// Used to compute the velocity of the radar
   FVector PrevLocation;
 
-  FVector2D CosSinIncrement;
-
   UWorld* World;
 
-  float LineTraceIncrement;
-
-  FVector debugCarVelocity = FVector::ZeroVector;
-
 };