Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Lidar performance #18

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Lidar performance #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
426f756
added initial version of load balancing in time of 3d lidar raycasts
cconti-rr Oct 15, 2021
1facbfe
changed 'alpha' to affect Value instead of transparency, but currentl…
cconti-rr Oct 18, 2021
6e7e79a
moved alpha
cconti-rr Oct 18, 2021
fb7552f
parallelized pointcloud2 message filling
cconti-rr Oct 22, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 18 additions & 2 deletions Source/RapyutaSimulationPlugins/Private/Sensors/BaseLidar.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,16 @@ void ABaseLidar::Scan()
checkNoEntry();
}

void ABaseLidar::ScanMultiframe()
{
checkNoEntry();
}

void ABaseLidar::DrawLidar()
{
checkNoEntry();
}

void ABaseLidar::LidarMessageUpdate(UROS2GenericMsg* TopicMessage)
{
checkNoEntry();
Expand Down Expand Up @@ -76,10 +86,16 @@ void ABaseLidar::GetData(TArray<FHitResult>& OutHits, float& OutTime)
OutTime = TimeOfLastScan;
}

FLinearColor ABaseLidar::GetColorFromIntensity(const float Intensity)
FLinearColor ABaseLidar::GetColorFromIntensity(const float Intensity, const float alpha)
{
// alpha doesn't seem to work with DrawPoints, so using alpha to change value (as in HSV)
float NormalizedIntensity = (Intensity - IntensityMin) / (IntensityMax - IntensityMin);
return InterpolateColor(NormalizedIntensity);
FLinearColor RetColor = InterpolateColor(NormalizedIntensity);
RetColor.R *= alpha;
RetColor.G *= alpha;
RetColor.B *= alpha;
//RetColor.A = alpha;
return RetColor;
}

FLinearColor ABaseLidar::InterpolateColor(float x)
Expand Down
228 changes: 179 additions & 49 deletions Source/RapyutaSimulationPlugins/Private/Sensors/SensorLidar3D.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,11 +61,75 @@ void ASensorLidar3D::Run()
TraceHandles.Init(FTraceHandle{}, NSamplesPerScan * NChannelsPerScan);
#endif

GetWorld()->GetGameInstance()->GetTimerManager().SetTimer(
TimerHandle, this, &ASensorLidar3D::Scan, 1.f / static_cast<float>(ScanFrequency), true);
if (SingleFrameScan)
{
GetWorld()->GetGameInstance()->GetTimerManager().SetTimer(
TimerHandle, this, &ASensorLidar3D::Scan, 1.f / static_cast<float>(ScanFrequency), true);
}
else
{
// assume fixed framerate of 100FPS - need to find a way to get this information not using DeltaTime
NSteps = 1.f / (.01f * ScanFrequency);
NSamplesPerStep = NSamplesPerScan * NChannelsPerScan / NSteps;
GetWorld()->GetGameInstance()->GetTimerManager().SetTimer(
TimerHandle, this, &ASensorLidar3D::ScanMultiframe, .01f, true);
}

IsInitialized = true;
}

void ASensorLidar3D::ScanMultiframe()
{
DHAngle = FOVHorizontal / static_cast<float>(NSamplesPerScan);
DVAngle = FOVVertical / static_cast<float>(NChannelsPerScan);

// complex collisions: true
FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
TraceParams.bReturnPhysicalMaterial = true;

// TraceParams.bIgnoreTouches = true;
TraceParams.bTraceComplex = true;
TraceParams.bReturnFaceIndex = true;

FVector lidarPos = GetActorLocation();
FRotator lidarRot = GetActorRotation();

ParallelFor(
NSamplesPerStep,
[this, &TraceParams, &lidarPos, &lidarRot](int32 Index)
{
const int32 GlobalIndex = Index + CurrentBatch * NSamplesPerStep;
const int IdxX = GlobalIndex % NSamplesPerScan;
const int IdxY = GlobalIndex / NSamplesPerScan;
const float HAngle = StartAngle + DHAngle * IdxX;
const float VAngle = StartVerticalAngle + DVAngle * IdxY;

FRotator laserRot(VAngle, HAngle, 0);
FRotator rot = UKismetMathLibrary::ComposeRotators(laserRot, lidarRot);

FVector startPos = lidarPos + MinRange * UKismetMathLibrary::GetForwardVector(rot);
FVector endPos = lidarPos + MaxRange * UKismetMathLibrary::GetForwardVector(rot);
// + WithNoise * FVector(GaussianRNGPosition(Gen),GaussianRNGPosition(Gen),GaussianRNGPosition(Gen));

GetWorld()->LineTraceSingleByChannel(
RecordedHits[Index], startPos, endPos, ECC_Visibility, TraceParams, FCollisionResponseParams::DefaultResponseParam);
},
false);

if (CurrentBatch == 0)
{
TimeOfLastScan = UGameplayStatics::GetTimeSeconds(GetWorld());
dt = 1.f / static_cast<float>(ScanFrequency);
}

if (ShowLidarRays)
{
DrawLidar();
}

CurrentBatch = (CurrentBatch + 1) % NSteps;
}

void ASensorLidar3D::Scan()
{
DHAngle = FOVHorizontal / static_cast<float>(NSamplesPerScan);
Expand Down Expand Up @@ -157,31 +221,46 @@ void ASensorLidar3D::Scan()
// need to store on a structure associating hits with time?
// GetROS2Data needs to get all data since the last Get? or the last within the last time interval?

if (ShowLidarRays)
{
DrawLidar();
}
}

void ASensorLidar3D::DrawLidar()
{
ULineBatchComponent* const LineBatcher = GetWorld()->PersistentLineBatcher;
if (LineBatcher != nullptr && ShowLidarRays)
if (LineBatcher != nullptr)
{
for (auto& h : RecordedHits)
// this can be maybe parallelized? a first trial with ParallelFor crashes - need to verify that LineBatchComponent is thread-safe
for (int i=0; i<RecordedHits.Num(); i++)
Comment on lines +235 to +236
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Probably the drawing itself must run in game thread, if in which case we could put LineBatcher->DrawPoint() to an async?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I haven't really investigated and is possibly low priority because in production mode we don't need to draw anyway
the current multipass approach can also be moved to async btw

{
auto& h = RecordedHits[i];
if (h.Actor != nullptr)
{
float Distance = (MinRange * (h.Distance > 0) + h.Distance) * .01f;

// coloring this way does not seem to work - looks as if CurrentBatch is constant over time
float alpha = 1;//(NSteps > 1) ? static_cast<float>((i/NSamplesPerStep + NSteps-1 - CurrentBatch) % NSteps) / static_cast<float>(NSteps) : 1;

if (h.PhysMaterial != nullptr)
{

// retroreflective material
if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1)
{
// UE_LOG(LogTemp, Warning, TEXT("retroreflective surface type hit"));
// LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorReflected, 10, .5, dt);
LineBatcher->DrawPoint(
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityReflective, Distance)), 5, 10, dt);
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityReflective, Distance), alpha), 5, 10, dt);
}
// non reflective material
else if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default)
{
// UE_LOG(LogTemp, Warning, TEXT("default surface type hit"));
// LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorHit, 10, .5, dt);
LineBatcher->DrawPoint(
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance)), 5, 10, dt);
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance), alpha), 5, 10, dt);
}
// reflective material
else if (h.PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2)
Expand All @@ -204,7 +283,7 @@ void ASensorLidar3D::Scan()
h.ImpactPoint,
GetColorFromIntensity(IntensityFromDist(
NormalAlignment * (IntensityReflective - IntensityNonReflective) + IntensityNonReflective,
Distance)),
Distance), alpha),
5,
10,
dt);
Expand All @@ -215,7 +294,7 @@ void ASensorLidar3D::Scan()
// UE_LOG(LogTemp, Warning, TEXT("no physics material"));
// LineBatcher->DrawLine(h.TraceStart, h.ImpactPoint, ColorHit, 10, .5, dt);
LineBatcher->DrawPoint(
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance)), 5, 10, dt);
h.ImpactPoint, GetColorFromIntensity(IntensityFromDist(IntensityNonReflective, Distance), alpha), 5, 10, dt);
}
}
else if (ShowLidarRayMisses)
Expand Down Expand Up @@ -342,53 +421,104 @@ FROSPointCloud2 ASensorLidar3D::GetROS2Data()
retValue.row_step = sizeof(float) * 5 * NSamplesPerScan;

retValue.data.Init(0, RecordedHits.Num() * sizeof(float) * 5);
for (auto i = 0; i < RecordedHits.Num(); i++)
{
float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f;
const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen);
float Intensity = 0;
if (RecordedHits.Last(i).PhysMaterial != nullptr)
ParallelFor(
RecordedHits.Num(),
[this,&retValue](int i)
{
// retroreflective material
if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1)
float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f;
const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen);
float Intensity = 0;
if (RecordedHits.Last(i).PhysMaterial != nullptr)
{
Intensity = IntensityScale * IntensityReflective;
}
// non-reflective material
else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default)
{
Intensity = IntensityScale * IntensityNonReflective;
// retroreflective material
if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1)
{
Intensity = IntensityScale * IntensityReflective;
}
// non-reflective material
else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default)
{
Intensity = IntensityScale * IntensityNonReflective;
}
// reflective material
else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2)
{
FVector HitSurfaceNormal = RecordedHits.Last(i).Normal;
FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart;
RayDirection.Normalize();

// the dot product for this should always be between 0 and 1
const float UnnormalizedIntensity =
FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) *
FVector::DotProduct(HitSurfaceNormal, -RayDirection),
IntensityNonReflective,
IntensityReflective);
check(UnnormalizedIntensity >= IntensityNonReflective);
check(UnnormalizedIntensity <= IntensityReflective);
Intensity = IntensityScale * UnnormalizedIntensity;
}
}
// reflective material
else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2)
else
{
FVector HitSurfaceNormal = RecordedHits.Last(i).Normal;
FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart;
RayDirection.Normalize();

// the dot product for this should always be between 0 and 1
const float UnnormalizedIntensity =
FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) *
FVector::DotProduct(HitSurfaceNormal, -RayDirection),
IntensityNonReflective,
IntensityReflective);
check(UnnormalizedIntensity >= IntensityNonReflective);
check(UnnormalizedIntensity <= IntensityReflective);
Intensity = IntensityScale * UnnormalizedIntensity;
Intensity = 0;//std::numeric_limits<float>::quiet_NaN();
}
}
else
{
Intensity = 0;//std::numeric_limits<float>::quiet_NaN();
}

FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f;
memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4);
memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4);
memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4);
memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4);
memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4);
}
FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f;
memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4);
memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4);
memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4);
memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4);
memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4);
},
false
);
// for (auto i = 0; i < RecordedHits.Num(); i++)
// {
// float Distance = (MinRange * (RecordedHits.Last(i).Distance > 0) + RecordedHits.Last(i).Distance) * .01f;
// const float IntensityScale = 1.f + WithNoise * GaussianRNGIntensity(Gen);
// float Intensity = 0;
// if (RecordedHits.Last(i).PhysMaterial != nullptr)
// {
// // retroreflective material
// if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType1)
// {
// Intensity = IntensityScale * IntensityReflective;
// }
// // non-reflective material
// else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType_Default)
// {
// Intensity = IntensityScale * IntensityNonReflective;
// }
// // reflective material
// else if (RecordedHits.Last(i).PhysMaterial->SurfaceType == EPhysicalSurface::SurfaceType2)
// {
// FVector HitSurfaceNormal = RecordedHits.Last(i).Normal;
// FVector RayDirection = RecordedHits.Last(i).TraceEnd - RecordedHits.Last(i).TraceStart;
// RayDirection.Normalize();

// // the dot product for this should always be between 0 and 1
// const float UnnormalizedIntensity =
// FMath::Clamp(IntensityNonReflective + (IntensityReflective - IntensityNonReflective) *
// FVector::DotProduct(HitSurfaceNormal, -RayDirection),
// IntensityNonReflective,
// IntensityReflective);
// check(UnnormalizedIntensity >= IntensityNonReflective);
// check(UnnormalizedIntensity <= IntensityReflective);
// Intensity = IntensityScale * UnnormalizedIntensity;
// }
// }
// else
// {
// Intensity = 0;//std::numeric_limits<float>::quiet_NaN();
// }

// FVector Pos = RecordedHits.Last(i).ImpactPoint * .01f;
// memcpy(&retValue.data[i * 4 * 5], &Pos.X, 4);
// memcpy(&retValue.data[i * 4 * 5 + 4], &Pos.Y, 4);
// memcpy(&retValue.data[i * 4 * 5 + 8], &Pos.Z, 4);
// memcpy(&retValue.data[i * 4 * 5 + 12], &Distance, 4);
// memcpy(&retValue.data[i * 4 * 5 + 16], &Intensity, 4);
// }

retValue.is_dense = true;

Expand Down
23 changes: 22 additions & 1 deletion Source/RapyutaSimulationPlugins/Public/Sensors/BaseLidar.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,12 @@ class RAPYUTASIMULATIONPLUGINS_API ABaseLidar : public AActor
UFUNCTION(BlueprintCallable)
virtual void Scan();

UFUNCTION(BlueprintCallable)
virtual void ScanMultiframe();

UFUNCTION()
virtual void DrawLidar();

UFUNCTION()
virtual void LidarMessageUpdate(UROS2GenericMsg* TopicMessage);

Expand Down Expand Up @@ -131,7 +137,22 @@ class RAPYUTASIMULATIONPLUGINS_API ABaseLidar : public AActor
UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Intensity")
float IntensityMax = 10000.f;

FLinearColor GetColorFromIntensity(const float Intensity);
FLinearColor GetColorFromIntensity(const float Intensity, const float alpha=1);



// this variable indicates the number of frames used to perform a full scan
UPROPERTY(VisibleAnywhere)
int32 NSteps = 1;

UPROPERTY(VisibleAnywhere)
int32 NSamplesPerStep = 0;

UPROPERTY(VisibleAnywhere)
int32 CurrentBatch = 0;

UPROPERTY(EditAnywhere)
bool SingleFrameScan = true;

protected:
float dt = 0.f;
Expand Down
4 changes: 4 additions & 0 deletions Source/RapyutaSimulationPlugins/Public/Sensors/SensorLidar3D.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,10 @@ class RAPYUTASIMULATIONPLUGINS_API ASensorLidar3D : public ABaseLidar

void Scan() override;

void ScanMultiframe() override;

void DrawLidar() override;

void LidarMessageUpdate(UROS2GenericMsg* TopicMessage) override;

bool Visible(AActor* TargetActor) override;
Expand Down