p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

WIP! Refactoring distribution of the rays.

This commit is contained in:
dotero 2019-12-09 12:29:06 +01:00 committed by Marc Garcia Puig
parent 617b5ae699
commit 158b5ed2cc
2 changed files with 64 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.cpp
View File

@ -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;
Steps = NewSteps;
PreCalculateCosSin();
PreCalculateLineTraceIncrement();
HorizontalFOV = NewHorizontalFOV;
}
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;
PreCalculateLineTraceIncrement();
}
void ARadar::SetPointLossPercentage(float NewLossPercentage)
{
PointLossPercentage = NewLossPercentage;
PointsPerSecond = NewPointsPerSecond;
RadarData.SetResolution(PointsPerSecond);
}
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;
FVector EndLocation = RadarLocation + WorldForwardVector;
FVector2D CurrentCosSin = {1.0f, 0.0f};
// Maximun radar radius in horizontal and vertical direction
const float MaxRx = FMath::Tan(FMath::DegreesToRadians(HorizontalFOV * 0.5f)) * Distance;
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)

36
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/Radar.h
View File

@ -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);
UFUNCTION(BlueprintCallable, Category = "Radar")
void SetPointLossPercentage(float NewLossPercentage);
void SetPointsPerSecond(int NewPointsPerSecond);
protected:
@ -54,27 +54,19 @@ protected:
float Distance;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
float FOV;
UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="Detection")
int Aperture;
float HorizontalFOV;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
int Steps;
float VerticalFOV;
/// Noise threshold [0.0, 1.0] of rays that will be
UPROPERTY(EditAnywhere, BlueprintReadOnly, Category="Detection")
float PointLossPercentage;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category="Detection")
int PointsPerSecond;
private:
void CalculateCurrentVelocity(const float DeltaTime);
void PreCalculateCosSin();
void PreCalculateLineTraceIncrement();
void SendLineTraces(float DeltaSeconds);
void SendLineTraces();
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;
};