RayCastRawLidar: First version, still in a very early stage.

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -34,6 +34,7 @@ class AInertialMeasurementUnit;
 class ALaneInvasionSensor;
 class AObstacleDetectionSensor;
 class ARadar;
+class ARayCastRawLidar;
 class ARayCastLidar;
 class ASceneCaptureCamera;
 class ASemanticSegmentationCamera;
@@ -59,6 +60,7 @@ namespace sensor {
     std::pair<ALaneInvasionSensor *, s11n::NoopSerializer>,
     std::pair<AObstacleDetectionSensor *, s11n::ObstacleDetectionEventSerializer>,
     std::pair<ARadar *, s11n::RadarSerializer>,
+    std::pair<ARayCastRawLidar *, s11n::LidarSerializer>,
     std::pair<ARayCastLidar *, s11n::LidarSerializer>,
     std::pair<ARssSensor *, s11n::NoopSerializer>,
     std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
@@ -83,6 +85,7 @@ namespace sensor {
 #include "Carla/Sensor/ObstacleDetectionSensor.h"
 #include "Carla/Sensor/Radar.h"
 #include "Carla/Sensor/RayCastLidar.h"
+#include "Carla/Sensor/RayCastRawLidar.h"
 #include "Carla/Sensor/RssSensor.h"
 #include "Carla/Sensor/SceneCaptureCamera.h"
 #include "Carla/Sensor/SemanticSegmentationCamera.h"

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

@@ -861,12 +861,17 @@ void UActorBlueprintFunctionLibrary::MakeLidarDefinition(
   DropOffAtZeroIntensity.Id = TEXT("dropoff_zero_intensity");
   DropOffAtZeroIntensity.Type = EActorAttributeType::Float;
   DropOffAtZeroIntensity.RecommendedValues = { TEXT("0.4") };
-
-  // Lower FOV limit.
+  // Noise in lidar cloud points.
   FActorVariation StdDevLidar;
   StdDevLidar.Id = TEXT("noise_stddev");
   StdDevLidar.Type = EActorAttributeType::Float;
   StdDevLidar.RecommendedValues = { TEXT("0.0") };
+  
+  if (Id == "ray_cast_raw") {
+    DropOffGenRate.RecommendedValues = { TEXT("0.0") };
+    DropOffIntensityLimit.RecommendedValues = { TEXT("1.0") };
+    DropOffAtZeroIntensity.RecommendedValues = { TEXT("0.0") };
+  }
 
   Definition.Variations.Append(
       {Channels, Range, PointsPerSecond, Frequency, UpperFOV, LowerFOV,

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

@@ -0,0 +1,242 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include <PxScene.h>
+#include <cmath>
+#include "Carla.h"
+#include "Carla/Sensor/RayCastRawLidar.h"
+#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
+#include "carla/geom/Math.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include "carla/geom/Math.h"
+#include <compiler/enable-ue4-macros.h>
+
+#include "DrawDebugHelpers.h"
+#include "Engine/CollisionProfile.h"
+#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
+#include "Runtime/Core/Public/Async/ParallelFor.h"
+
+FActorDefinition ARayCastRawLidar::GetSensorDefinition()
+{
+  return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast_raw"));
+}
+
+ARayCastRawLidar::ARayCastRawLidar(const FObjectInitializer& ObjectInitializer)
+  : Super(ObjectInitializer)
+{
+  PrimaryActorTick.bCanEverTick = true;
+
+  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
+}
+
+void ARayCastRawLidar::Set(const FActorDescription &ActorDescription)
+{
+  Super::Set(ActorDescription);
+  FLidarDescription LidarDescription;
+  UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
+  Set(LidarDescription);
+}
+
+void ARayCastRawLidar::Set(const FLidarDescription &LidarDescription)
+{
+  Description = LidarDescription;
+  LidarMeasurement = FLidarMeasurement(Description.Channels);
+  CreateLasers();
+}
+
+void ARayCastRawLidar::CreateLasers()
+{
+  const auto NumberOfLasers = Description.Channels;
+  check(NumberOfLasers > 0u);
+  const float DeltaAngle = NumberOfLasers == 1u ? 0.f :
+    (Description.UpperFovLimit - Description.LowerFovLimit) /
+    static_cast<float>(NumberOfLasers - 1);
+  LaserAngles.Empty(NumberOfLasers);
+  for(auto i = 0u; i < NumberOfLasers; ++i)
+  {
+    const float VerticalAngle =
+        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 ARayCastRawLidar::Tick(const float DeltaTime)
+{
+  Super::Tick(DeltaTime);
+
+  ReadPoints(DeltaTime);
+
+  auto DataStream = GetDataStream(*this);
+  DataStream.Send(*this, LidarMeasurement, DataStream.PopBufferFromPool());
+}
+
+void ARayCastRawLidar::ReadPoints(const float DeltaTime)
+{
+  const uint32 ChannelCount = Description.Channels;
+  const uint32 PointsToScanWithOneLaser =
+    FMath::RoundHalfFromZero(
+        Description.PointsPerSecond * DeltaTime / float(ChannelCount));
+
+  if (PointsToScanWithOneLaser <= 0)
+  {
+    UE_LOG(
+        LogCarla,
+        Warning,
+        TEXT("%s: no points requested this frame, try increasing the number of points per second."),
+        *GetName());
+    return;
+  }
+
+  check(ChannelCount == LaserAngles.Num());
+
+  const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
+      LidarMeasurement.GetHorizontalAngle());
+  const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime;
+  const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
+
+  LidarMeasurement.Reset(ChannelCount, PointsToScanWithOneLaser);
+
+
+  GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
+  ParallelFor(ChannelCount, [&](int32 idxChannel) {
+
+    FCriticalSection Mutex;
+    ParallelFor(PointsToScanWithOneLaser, [&](int32 idxPtsOneLaser) {
+      FVector Point;
+      float Intensity;
+      const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * idxPtsOneLaser;
+      if (ShootLaser(idxChannel, Angle, Point, Intensity)) {
+        Mutex.Lock();
+        LidarMeasurement.WritePointAsync(idxChannel, {Point, Intensity});
+        Mutex.Unlock();
+      }
+    });
+  });
+  GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
+
+  LidarMeasurement.SaveDetections();
+
+
+  const float HorizontalAngle = carla::geom::Math::ToRadians(
+      std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f));
+  LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
+}
+
+float ARayCastRawLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const
+{
+  return 0.0;
+
+  const FVector HitPoint = HitInfo.ImpactPoint - LidarBodyLoc;
+  const float Distance = 0.01f * HitPoint.Size();
+
+  const float AttenAtm = Description.AtmospAttenRate;
+  const float AbsAtm = exp(-AttenAtm * Distance);
+
+  const FActorRegistry &Registry = GetEpisode().GetActorRegistry();
+
+  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;
+}
+
+bool ARayCastRawLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ, float &Intensity) const
+{
+
+  if(DropOffGenActive && RandomEngine->GetUniformFloat() < Description.DropOffGenRate)
+    return false;
+
+  const float VerticalAngle = LaserAngles[Channel];
+
+  FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
+  TraceParams.bTraceComplex = true;
+  TraceParams.bReturnPhysicalMaterial = false;
+
+  FHitResult HitInfo(ForceInit);
+
+  FTransform actorTransf = GetTransform();
+  FVector LidarBodyLoc = GetActorLocation();
+  FRotator LidarBodyRot = GetActorRotation();
+  FRotator LaserRot (VerticalAngle, HorizontalAngle, 0);  // float InPitch, float InYaw, float InRoll
+  FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
+    LaserRot,
+    LidarBodyRot
+  );
+  const auto Range = Description.Range;
+  FVector EndTrace = Range * UKismetMathLibrary::GetForwardVector(ResultRot) + LidarBodyLoc;
+
+  GetWorld()->LineTraceSingleByChannel(
+    HitInfo,
+    LidarBodyLoc,
+    EndTrace,
+    ECC_GameTraceChannel2,
+    TraceParams,
+    FCollisionResponseParams::DefaultResponseParam
+  );
+
+
+  if (HitInfo.bBlockingHit)
+  {
+    if (Description.ShowDebugPoints)
+    {
+      DrawDebugPoint(
+        GetWorld(),
+        HitInfo.ImpactPoint,
+        10,  //size
+        FColor(255,0,255),
+        false,  //persistent (never goes away)
+        0.1  //point leaves a trail on moving object
+      );
+    }
+
+    const FVector hp = HitInfo.ImpactPoint;
+    XYZ = actorTransf.Inverse().TransformPosition(hp);
+
+    Intensity = ComputeIntensity(LidarBodyLoc, HitInfo);
+
+
+
+    if(Intensity > Description.DropOffIntensityLimit)
+      return true;
+    else
+      return RandomEngine->GetUniformFloat() < DropOffAlpha * Intensity + DropOffBeta;
+
+  } else {
+    return false;
+  }
+}

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

@@ -0,0 +1,73 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "Carla/Sensor/Sensor.h"
+
+#include "Carla/Actor/ActorDefinition.h"
+#include "Carla/Sensor/LidarDescription.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include <carla/sensor/s11n/LidarMeasurement.h>
+#include <compiler/enable-ue4-macros.h>
+
+#include "RayCastRawLidar.generated.h"
+
+/// A ray-cast based Lidar sensor.
+UCLASS()
+class CARLA_API ARayCastRawLidar : public ASensor
+{
+  GENERATED_BODY()
+
+  using FLidarMeasurement = carla::sensor::s11n::LidarMeasurement;
+
+public:
+
+  static FActorDefinition GetSensorDefinition();
+
+  ARayCastRawLidar(const FObjectInitializer &ObjectInitializer);
+
+  void Set(const FActorDescription &Description) override;
+
+  void Set(const FLidarDescription &LidarDescription);
+
+protected:
+
+  virtual void Tick(float DeltaTime) override;
+
+private:
+
+  /// Creates a Laser for each channel.
+  void CreateLasers();
+
+  /// Updates LidarMeasurement with the points read in DeltaTime.
+  void ReadPoints(float DeltaTime);
+
+  /// Shoot a laser ray-trace, return whether the laser hit something.
+  bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point, float& Intensity) const;
+
+  /// Compute the received intensity of the point
+  float ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const;
+
+  UPROPERTY(EditAnywhere)
+  FLidarDescription Description;
+
+  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;
+};