CARLA
RayCastLidar.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include <PxScene.h>
8 #include <cmath>
9 #include "Carla.h"
12 #include "carla/geom/Math.h"
13 
15 #include "carla/geom/Math.h"
16 #include "carla/geom/Location.h"
18 
19 #include "DrawDebugHelpers.h"
20 #include "Engine/CollisionProfile.h"
21 #include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
22 
24 {
26 }
27 
28 
29 ARayCastLidar::ARayCastLidar(const FObjectInitializer& ObjectInitializer)
30  : Super(ObjectInitializer) {
31 
32  RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
34 }
35 
36 void ARayCastLidar::Set(const FActorDescription &ActorDescription)
37 {
38  ASensor::Set(ActorDescription);
39  FLidarDescription LidarDescription;
40  UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
41  Set(LidarDescription);
42 }
43 
44 void ARayCastLidar::Set(const FLidarDescription &LidarDescription)
45 {
46  Description = LidarDescription;
48  CreateLasers();
50 
51  // Compute drop off model parameters
54  DropOffGenActive = Description.DropOffGenRate > std::numeric_limits<float>::epsilon();
55 }
56 
57 void ARayCastLidar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
58 {
59  TRACE_CPUPROFILER_EVENT_SCOPE(ARayCastLidar::PostPhysTick);
60  SimulateLidar(DeltaTime);
61 
62  {
63  TRACE_CPUPROFILER_EVENT_SCOPE_STR("Send Stream");
64  auto DataStream = GetDataStream(*this);
65  DataStream.Send(*this, LidarData, DataStream.PopBufferFromPool());
66  }
67 }
68 
69 float ARayCastLidar::ComputeIntensity(const FSemanticDetection& RawDetection) const
70 {
71  const carla::geom::Location HitPoint = RawDetection.point;
72  const float Distance = HitPoint.Length();
73 
74  const float AttenAtm = Description.AtmospAttenRate;
75  const float AbsAtm = exp(-AttenAtm * Distance);
76 
77  const float IntRec = AbsAtm;
78 
79  return IntRec;
80 }
81 
82 ARayCastLidar::FDetection ARayCastLidar::ComputeDetection(const FHitResult& HitInfo, const FTransform& SensorTransf) const
83 {
84  FDetection Detection;
85  const FVector HitPoint = HitInfo.ImpactPoint;
86  Detection.point = SensorTransf.Inverse().TransformPosition(HitPoint);
87 
88  const float Distance = Detection.point.Length();
89 
90  const float AttenAtm = Description.AtmospAttenRate;
91  const float AbsAtm = exp(-AttenAtm * Distance);
92 
93  const float IntRec = AbsAtm;
94 
95  Detection.intensity = IntRec;
96 
97  return Detection;
98 }
99 
100  void ARayCastLidar::PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel) {
101  Super::PreprocessRays(Channels, MaxPointsPerChannel);
102 
103  for (auto ch = 0u; ch < Channels; ch++) {
104  for (auto p = 0u; p < MaxPointsPerChannel; p++) {
106  }
107  }
108  }
109 
111  {
112  if (Description.NoiseStdDev > std::numeric_limits<float>::epsilon()) {
113  const auto ForwardVector = Detection.point.MakeUnitVector();
114  const auto Noise = ForwardVector * RandomEngine->GetNormalDistribution(0.0f, Description.NoiseStdDev);
115  Detection.point += Noise;
116  }
117 
118  const float Intensity = Detection.intensity;
119  if(Intensity > Description.DropOffIntensityLimit)
120  return true;
121  else
122  return RandomEngine->GetUniformFloat() < DropOffAlpha * Intensity + DropOffBeta;
123  }
124 
125  void ARayCastLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
126  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
127  PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
128 
130 
131  for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
132  for (auto& hit : RecordedHits[idxChannel]) {
133  FDetection Detection = ComputeDetection(hit, SensorTransform);
134  if (PostprocessDetection(Detection))
135  LidarData.WritePointSync(Detection);
136  else
137  PointsPerChannel[idxChannel]--;
138  }
139  }
140 
142  }
float DropOffGenRate
General drop off rate.
static FActorDefinition MakeLidarDefinition(const FString &Id)
void PreprocessRays(uint32_t Channels, uint32_t MaxPointsPerChannel) override
Method that allow to preprocess if the rays will be traced.
std::vector< std::vector< FHitResult > > RecordedHits
float DropOffIntensityLimit
General drop off rate.
virtual void ResetMemory(std::vector< uint32_t > points_per_channel)
Definition: LidarData.h:82
virtual void PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime)
virtual void WriteChannelCount(std::vector< uint32_t > points_per_channel)
void SetSeed(int32 InSeed)
Definition: Sensor.cpp:72
static FActorDefinition GetSensorDefinition()
Helper class to store and serialize the data generated by a RawLidar.
void CreateLasers()
Creates a Laser for each channel.
ARayCastLidar(const FObjectInitializer &ObjectInitializer)
void SimulateLidar(const float DeltaTime)
Updates LidarMeasurement with the points read in DeltaTime.
A definition of a Carla Actor with all the variation and attributes.
FLidarDescription Description
float Length() const
Definition: geom/Vector3D.h:49
FDetection ComputeDetection(const FHitResult &HitInfo, const FTransform &SensorTransf) const
FLidarData LidarData
Definition: RayCastLidar.h:50
carla::sensor::data::LidarData FLidarData
Definition: RayCastLidar.h:28
static void SetLidar(const FActorDescription &Description, FLidarDescription &Lidar)
A description of a Carla Actor with all its variation.
std::vector< std::vector< bool > > RayPreprocessCondition
float AtmospAttenRate
Attenuation Rate in the atmosphere in m^-1.
bool PostprocessDetection(FDetection &Detection) const
uint32 Channels
Number of lasers.
float DropOffAtZeroIntensity
General drop off rate.
float ComputeIntensity(const FSemanticDetection &RawDetection) const
Compute the received intensity of the point.
float DropOffBeta
Definition: RayCastLidar.h:61
float GetUniformFloat()
Definition: RandomEngine.h:56
float DropOffAlpha
Slope for the intensity dropoff of lidar points, it is calculated throught the dropoff limit and the ...
Definition: RayCastLidar.h:60
void WritePointSync(LidarDetection &detection)
Definition: LidarData.h:93
bool DropOffGenActive
Enable/Disable general dropoff of lidar points.
Definition: RayCastLidar.h:53
FAsyncDataStream GetDataStream(const SensorT &Self)
Return the FDataStream associated with this sensor.
int RandomSeed
Random seed for the noise/dropoff used by this sensor.
float GetNormalDistribution(float Mean, float StandardDeviation)
Definition: RandomEngine.h:110
Vector3D MakeUnitVector() const
Definition: geom/Vector3D.h:57
void ComputeAndSaveDetections(const FTransform &SensorTransform) override
This method uses all the saved FHitResults, compute the RawDetections and then send it to the LidarDa...
std::vector< uint32_t > PointsPerChannel
Helper class to store and serialize the data generated by a Lidar.
Definition: LidarData.h:47
virtual void Set(const FActorDescription &Description)
Definition: Sensor.cpp:33
virtual void Set(const FActorDescription &Description) override
URandomEngine * RandomEngine
Random Engine used to provide noise for sensor output.