Lidar.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "Lidar.h"
 
#include "LidarNoiseModel.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Utils/PointcloudUtilities.h"
#include "Agrarsense/Utils/PlatformUtilities.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Sensor/Lidar/LidarManager.h"
#include "Agrarsense/Sensor/Lidar/PointData.h"
#include "Agrarsense/Weather/Weather.h"
 
#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
#include "NiagaraDataInterfaceArrayFunctionLibrary.h"
#include "Math/UnrealMathUtility.h"
#include "NiagaraFunctionLibrary.h"
#include <Misc/FileHelper.h>
#include "NiagaraComponent.h"
#include "Components/PrimitiveComponent.h"
#include "Landscape.h"
#include "Async/ParallelFor.h"
#include "Engine/GameViewportClient.h"
#include "Engine/Engine.h"
 
#include <iterator>
 
ALidar::ALidar(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    // Lidar sensors are ticked by LidarManager.cpp
    PrimaryActorTick.bCanEverTick = false;
}
 
void ALidar::Init(FLidarParameters parameters, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
    LidarParameters = parameters;
    ChangeParameters();
    CreateDataSavePath();
 
    // Initialize PointCloudMessage
    PointCloudMessage.Reset();
    PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
    PointCloudMessage->header.frame_id = "world";
    PointCloudMessage->is_dense = true;
    PointCloudMessage->fields.SetNum(4);
    PointCloudMessage->height = 1;
 
    PointCloudMessage->fields[0].name = "x";
    PointCloudMessage->fields[0].offset = 0;
    PointCloudMessage->fields[0].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[0].count = 1;
 
    PointCloudMessage->fields[1].name = "y";
    PointCloudMessage->fields[1].offset = 4;
    PointCloudMessage->fields[1].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[1].count = 1;
 
    PointCloudMessage->fields[2].name = "z";
    PointCloudMessage->fields[2].offset = 8;
    PointCloudMessage->fields[2].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::FLOAT32;
    PointCloudMessage->fields[2].count = 1;
 
    PointCloudMessage->fields[3].name = "rgb";
    PointCloudMessage->fields[3].offset = 12;
    PointCloudMessage->fields[3].datatype = ROSMessages::sensor_msgs::PointCloud2::PointField::UINT32;
    PointCloudMessage->fields[3].count = 1;
 
    PointCloudMessage->point_step = 16;
 
    Generator.seed(RandomDevice());
}
 
void ALidar::BeginPlay()
{
    Super::BeginPlay();
 
    World = GetWorld();
 
    // Create the SemanticColors TMap
    TMap<FString, FColor> Colors = GetSemanticColors();
    for (const auto& ColorEntry : Colors)
    {
        FName SemanticName = FName(*ColorEntry.Key);
        SemanticColors.Add(SemanticName, ColorEntry.Value);
    }
 
    // Cache "None" and "Snowflake" colors for Semantic
    FColor NoneColor = SemanticColors.FindRef("None");
    FColor SnowflakeColor = SemanticColors.FindRef("Snowflake");
    CachedNoneColor = (NoneColor.R << 16) | (NoneColor.G << 8) | (NoneColor.B << 0);
    CachedSnowflakeColor = (SnowflakeColor.R << 16) | (SnowflakeColor.G << 8) | (SnowflakeColor.B << 0);
 
    // Add this lidar to LidarManager that handles ticking of this sensor in parallel.
    ALidarManager* LidarManager = UAgrarsenseStatics::GetLidarManager(World);
    if (LidarManager)
    {
        LidarManager->AddRaycastLidar(this);
    }
 
    AWeather* Weather = UAgrarsenseStatics::GetWeatherActor(World);
    if (Weather)
    {
        CurrentWeatherParameters = Weather->GetCurrentWeather();
        Weather->OnWeatherChanged.AddUniqueDynamic(this, &ALidar::OnWeatherChanged);
    }
 
    // Load pointcloud Niagara particle system
    UNiagaraSystem* NS = LoadObject<UNiagaraSystem>(nullptr, TEXT("/Game/Agrarsense/Particles/PointCloud/NS_PointCloudSemanticFast"), nullptr, LOAD_None, nullptr);
    if (NS)
    {
        NiagaraComponent = UNiagaraFunctionLibrary::SpawnSystemAtLocation(World, NS, GetActorLocation());
        if (NiagaraComponent)
        {
            // Hide this NiagaraComponent for all Camera sensors.
            HideComponentForAllCameras(Cast<UPrimitiveComponent>(NiagaraComponent));
 
            NiagaraComponent->SetAffectDynamicIndirectLighting(false);
            NiagaraComponent->SetAffectDistanceFieldLighting(false);
            NiagaraComponent->SetCastContactShadow(false);
            NiagaraComponent->SetCastShadow(false);
        }
    }
#if WITH_EDITOR
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("Lidar.cpp: Couldn't find Niagara particle system."));
    }
#endif
}
 
void ALidar::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    World = GetWorld();
 
    // Remove this lidar from LidarManager
    ALidarManager* LidarManager = UAgrarsenseStatics::GetLidarManager(World);
    if (LidarManager)
    {
        LidarManager->RemoveRaycastLidar(this);
    }
 
    // Unbind from Weather changed event
    AWeather* Weather = UAgrarsenseStatics::GetWeatherActor(World);
    if (Weather)
    {
        Weather->OnWeatherChanged.RemoveDynamic(this, &ALidar::OnWeatherChanged);
    }
 
    if (NiagaraComponent)
    {
        SetNiagaraRendering(false);
 
        // Destroy component
        NiagaraComponent->UnregisterComponent();
        NiagaraComponent->DestroyComponent();
        NiagaraComponent = nullptr;
    }
 
    PointCloudMessage.Reset();
 
    World = nullptr;
 
    // Clear all containers on EndPlay
    PreProcessedHitImpactPoints.clear();
    PointsPerChannel.clear();
    PointsFlattened.clear();
    LaserAngles.clear();
    Points.clear();
}
 
void ALidar::OnWeatherChanged(FWeatherParameters WeatherParams)
{
    CurrentWeatherParameters = WeatherParams;
 
    // Here we recalculate SnowHeightOffset when Weather changes.
    // Amount is fine-tuned by hand. If the Snow implementation would change meaningfully,
    // we would need to change Amount value too.
    // See AddProcessingToHitResult() function how SnowHeightOffset is used.
    const float Amount = 0.1f;
    SnowHeightOffset = Amount * (CurrentWeatherParameters.SnowAmount / 100.0f);
}
 
void ALidar::ChangeParameters()
{
    // Ensure certain parameters are valid ranges
    LidarParameters.HorizontalFov = FMath::Clamp(LidarParameters.HorizontalFov, 0.1f, 360.0f);
    LidarParameters.Channels = FMath::Max(LidarParameters.Channels, 1);
    LidarParameters.DistanceNoiseStdDev = FMath::Clamp(LidarParameters.DistanceNoiseStdDev, 0.0f, 1.0f);
    LidarParameters.LateralNoiseStdDev = FMath::Clamp(LidarParameters.LateralNoiseStdDev, 0.0f, 1.0f);
 
    const int32 NumberOfLasers = LidarParameters.Channels;
 
    // Resize arrays
    PreProcessedHitImpactPoints.resize(NumberOfLasers);
    Points.resize(NumberOfLasers);
 
    // Create Lidar linetrace angles
    const float DeltaAngle = NumberOfLasers == 1u ? 0.f : (LidarParameters.UpperFovLimit - LidarParameters.LowerFovLimit) / static_cast<float>(NumberOfLasers - 1);
    LaserAngles.clear();
 
    for (int32 i = 0u; i < NumberOfLasers; ++i)
    {
        const float VerticalAngle = LidarParameters.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
        LaserAngles.emplace_back(VerticalAngle);
    }
 
    SendDataToROS = LidarParameters.SendDataToROS;
    SensorHzFrequency = 1.0f / LidarParameters.RotationFrequency;
    SendDataEveryFrame = !LidarParameters.SendDataAtRotationFrequency;
    
 
    if (LidarParameters.PointsPerSecond > 1000000)
    {
        SimulatorLog::Log("Lidar sensor: It's highly advisable to turn off VisualizePointcloud when PointsPerSecond exceed 1 million.");
 
        if (LidarParameters.PointsPerSecond > 2000000)
        {
            LidarParameters.VisualizePointcloud = false;
            SimulatorLog::Log("Lidar sensor: VisualizePointcloud has been set to false due PointsPerSecond exceeding 2 million.");
        }
    }
 
    SetVisualizeLidarParticles(LidarParameters.VisualizePointcloud);
 
    CreateROSTopic();
}
 
void ALidar::SetVisualizeLidarParticles(bool Visualize)
{
    SetNiagaraRendering(Visualize);
}
 
void ALidar::SetNiagaraRendering(bool Enabled)
{
    if (NiagaraComponent)
    {
        if (!Enabled)
        {
            TArray<FVector> EmptyHitpoints;
            TArray<FLinearColor> EmptyColors;
            UpdateLidarParticles(0, EmptyHitpoints, EmptyColors);
        }
 
        NiagaraComponent->SetRenderingEnabled(Enabled);
    }
}
 
void ALidar::UpdateLidarParticles(int NumberOfHits, const TArray<FVector>& HitPoints, const TArray<FLinearColor>& Colors)
{
    if (NiagaraComponent)
    {
        NiagaraComponent->SetVariableFloat(NiagaraPointsFloat, float(NumberOfHits));
        NiagaraComponent->SetVariableInt(NiagaraPointsInt, NumberOfHits);
        UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayVector(NiagaraComponent, NiagaraHitPoints, HitPoints);
        UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayColor(NiagaraComponent, NiagaraHitColors, Colors);
    }
}
 
void ALidar::SetParticleLifeTime(float ParticleLifeTime)
{
    if (NiagaraComponent)
    {
        NiagaraComponent->SetVariableFloat(FName("User.LifeTime"), ParticleLifeTime);
    }
}
 
 std::vector<FPointData> ALidar::SimulateRaycastLidar(const float DeltaTime)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::SimulateRaycastLidar);
 
    std::vector<FPointData> HitPoints;
 
    if (!CanSimulateSensor())
    {
        return HitPoints;
    }
 
    // Get sensor position and rotation
    const FTransform actorTransform = GetTransform();
    const FVector LidarBodyLocation = actorTransform.GetLocation();
    const FRotator LidarBodyRotation = actorTransform.Rotator();
    const FQuat LidarBodyQuat = FQuat(LidarBodyRotation);
 
    // Prepare needed variables for processing
    const int32 ChannelCount = LidarParameters.Channels;
    const int32 PointsToScanWithOneChannel = FMath::RoundHalfFromZero(LidarParameters.PointsPerSecond * DeltaTime / float(ChannelCount));
    const float HorizontalAngle = CurrentHorizontalAngle;
    const float AngleDistanceOfTick = LidarParameters.RotationFrequency * LidarParameters.HorizontalFov * DeltaTime;
    const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneChannel;
    const float Range = LidarParameters.Range * 100; // to meters
 
    const FColor Color = FColor(255, 255, 255);
    const int32 RgbDefault = (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
 
    FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
    TraceParams.bReturnPhysicalMaterial = false;
 
    // Note. can become very expensive with high vertice meshes.
    // Any very high vertice mesh should have simpler collision mesh (Tree for example)
    TraceParams.bTraceComplex = LidarParameters.UseComplexCollisionTrace;
 
    // Check if we are going to add any noise
    const bool ShouldAddNoise = (LidarParameters.UseLidarNoiseModel && CurrentWeatherParameters.IsLidarNoiseModelCondition())
        || LidarParameters.DistanceNoiseStdDev != 0.0f
        || LidarParameters.LateralNoiseStdDev != 0.0f;
 
    // Check if separate point cloud should be saved without the noise
    NeedToSavePointCloudWithoutNoise = ShouldAddNoise
        && LidarParameters.SaveDataToDisk 
        && LidarParameters.SavePointcloudWithoutNoiseModel;
 
    const bool Semantic = LidarParameters.Semantic;
    const float HorizontalFOV = LidarParameters.HorizontalFov;
    const float HorizontalFOVHalf = HorizontalFOV / 2;
    
    ResetRecordedHits(PointsToScanWithOneChannel);
 
    // Loop Lidar channels in Parallel
    ParallelFor(ChannelCount, [&](int32 Channel)
    {
        const float VerticalAngle = LaserAngles[Channel];
        int32 RGB = RgbDefault;
 
        FRotator LaserRotation;
        FHitResult HitResult;
        FVector EndTrace;
 
        // Generate random bool which we use later in AddLateralNoise
        bool UseHorizontalNoise = FMath::RandBool();
 
        // Cache the reference to Points[idxChannel]
        std::vector<FPointData>& ChannelPoints = Points[Channel];
 
        // Loop through this channel linetraces
        for (int32 i = 0; i < PointsToScanWithOneChannel; i++)
        {
            const float LaserHorizontalAngle = std::fmod(HorizontalAngle + AngleDistanceOfLaserMeasure * i, HorizontalFOV) - HorizontalFOVHalf;
 
            HitResult.Reset();
 
            // Calculate the end trace point for this linetrace
            LaserRotation = FRotator(VerticalAngle, LaserHorizontalAngle, 0);
            FQuat LaserQuat = FQuat(LaserRotation);
            FQuat ResultQuat = LaserQuat * LidarBodyQuat;
            EndTrace = Range * ResultQuat.Vector() + LidarBodyLocation;
 
            if (ShootLaser(HitResult, EndTrace, TraceParams, LidarBodyLocation, Channel, Semantic, RgbDefault))
            {
                if (Semantic)
                {
                    RGB = GetSemanticData(HitResult);
                }
 
                AddProcessingToHitResult(HitResult, LidarBodyLocation, UseHorizontalNoise);
 
                // Add HitResult impact point to the current channel's point list
                const FVector& HitPoint = HitResult.ImpactPoint;
                ChannelPoints.emplace_back(HitPoint.X, HitPoint.Y, HitPoint.Z, RGB);
            }
        }
    }, EParallelForFlags::Unbalanced);
 
    CurrentHorizontalAngle = FMath::Fmod(HorizontalAngle + AngleDistanceOfTick, LidarParameters.HorizontalFov);
 
    SendData(DeltaTime);
 
    if (LidarParametersChanged)
    {
        LidarParameters = TempLidarParameters;
        LidarParametersChanged = false;
        ChangeParameters();
    }
 
    if (ClearContainers && LidarParameters.SendDataToCombinedROSTopic)
    {
        return PointsFlattened;
    }
 
    return HitPoints;
}
 
void ALidar::AddProcessingToHitResult(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise)
{
    AddSnowTerrainAdjustment(HitResult, LidarBodyLocation);
    AddDistanceNoise(HitResult, LidarBodyLocation);
    AddLateralNoise(HitResult, LidarBodyLocation, UseHorizontalNoise);
}
 
void ALidar::AddDistanceNoise(FHitResult& HitResult, const FVector& LidarBodyLocation)
{
    if (LidarParameters.DistanceNoiseStdDev > 0.0f)
    {
        std::normal_distribution<float> Distribution(0.0f, LidarParameters.DistanceNoiseStdDev);
        float NoiseValue = Distribution(Generator);
        const FVector DirectionToSensor = ((LidarBodyLocation * 1e-2) - HitResult.ImpactPoint).GetSafeNormal();
        const FVector Noise = DirectionToSensor * NoiseValue;
        HitResult.ImpactPoint += Noise;
    }
}
 
void ALidar::AddLateralNoise(FHitResult& HitResult, const FVector& LidarBodyLocation, const bool UseHorizontalNoise)
{
    if (LidarParameters.LateralNoiseStdDev > 0.0f)
    {
        const FVector ForwardVector = (HitResult.ImpactPoint - LidarBodyLocation).GetSafeNormal();
        const FVector RightVector = FVector::CrossProduct(ForwardVector, FVector::UpVector).GetSafeNormal();
 
        FVector ChosenDirection;
        if (UseHorizontalNoise)
        {
            ChosenDirection = RightVector;
        }
        else
        {
            FVector UpVector = FVector::CrossProduct(ForwardVector, RightVector).GetSafeNormal();
            ChosenDirection = UpVector;
        }
 
        std::normal_distribution<float> Distribution(0.0f, LidarParameters.LateralNoiseStdDev);
        const float NoiseValue = Distribution(Generator);
 
        HitResult.ImpactPoint += ChosenDirection * NoiseValue;
    }
}
 
void ALidar::AddSnowTerrainAdjustment(FHitResult& HitResult, const FVector& LidarBodyLocation)
{
    if (LidarParameters.UseTerrainSnowHitAdjustment && CurrentWeatherParameters.IsWinterSnowCondition())
    {
        bool HitTerrain = false;
        AActor* HitActor = HitResult.GetActor();
 
        // Terrain should have tag "Terrain" in order to make this work.
        if (HitActor && HitActor->Tags.Contains("Terrain"))
        {
            HitTerrain = true;
        }
 
        if (HitTerrain)
        {
            // Here we adjust HitResult ImpactPoint if Linetrace hits "Terrain" and there is Snow.
            // This is done because the simulator snow implementation
            // is using World Position Offset in material, meaning it has no actual collider where linetrace could hit.
            // Thus we need to manually calculate the rough point where Lidar linetrace goes through the snow layer.
 
            // ImpactPoint is adjusted based on the SnowHeightOffset calculated during OnWeatherChanged function.
            // SnowHeightOffset represents the height at which the Linetrace goes through the snow layer.
            // We use linear interpolation (Lerp) to smoothly adjust the ImpactPoint towards LidarBodyLocation.
            FVector NewHitPoint = FMath::Lerp(HitResult.ImpactPoint, (LidarBodyLocation * 1e-2), SnowHeightOffset);
            HitResult.ImpactPoint = NewHitPoint;
        }
    }
}
 
void ALidar::VisualizeLidarParticles()
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::VisualizeLidarParticles);
 
    if (!LidarParameters.VisualizePointcloud || !NiagaraComponent)
    {
        return;
    }
 
    if (GEngine
        && GEngine->GameViewport
        && GEngine->GameViewport->bDisableWorldRendering)
    {
        // If world rendering (Spectator camera) is disabled, no need to update the particle system.
        return;
    }
 
    int32 NumberOfHits = PointsFlattened.size();
    if (NumberOfHits != 0)
    {
        // Convert std::vector<FPointData> to 
        // TArray<FVector> and TArray<FLinearColor> for Niagara visualization.
 
        // Create and allocate HitLocations TArray
        TArray<FVector> HitLocations;
        HitLocations.SetNumUninitialized(NumberOfHits);
 
        // Create and set initial colors to green
        TArray<FLinearColor> Colors;
        Colors.Init(FLinearColor::Green, NumberOfHits);
 
        // Pointer access for reduced overhead
        FVector* HitLocationPtr = HitLocations.GetData();
        FLinearColor* ColorsPtr = Colors.GetData();
 
        const bool Semantic = LidarParameters.Semantic;
 
        for (int32 i = 0; i < NumberOfHits; ++i)
        {
            const FPointData& point = PointsFlattened[i];
 
            HitLocationPtr[i] = FVector(point.X, (point.Y * -1), point.Z);
 
            if (Semantic)
            {
                // Convert point.RGB (uint32) to FLinearColor
                uint8 R = (point.RGB >> 16) & 0xFF;
                uint8 G = (point.RGB >> 8) & 0xFF;
                uint8 B = point.RGB & 0xFF;
                ColorsPtr[i] = FLinearColor(R / 255.0f, G / 255.0f, B / 255.0f);
            }
        }
        UpdateLidarParticles(NumberOfHits, HitLocations, Colors);
    }
}
 
void ALidar::SendData(const float DeltaTime)
{
    if (!CanSendData(DeltaTime))
    {
        return;
    }
 
    // Pre allocate size to PointsFlattened
    size_t totalSize = 0;
    for (const auto& vec : Points)
    {
        totalSize += vec.size();
    }
    PointsFlattened.reserve(totalSize);
 
    // Fill PointsFlattened array
    const int32 channelCount = LidarParameters.Channels;
    for (int32 idxChannel = 0; idxChannel < channelCount; ++idxChannel)
    {
        PointsFlattened.insert(PointsFlattened.end(), std::make_move_iterator(Points[idxChannel].begin()), std::make_move_iterator(Points[idxChannel].end()));
    }
 
    if (LidarParameters.SaveDataToDisk)
    {
        SaveDataToDisk();
    }
 
    if (SendDataToROS)
    {
        SendDataToTopic(PointsFlattened);
    }
}
 
bool ALidar::CanSendData(const float DeltaTime)
{
    if (SendDataEveryFrame)
    {
        ClearContainers = true;
        return true;
    }
 
    SensorHzTimer += DeltaTime;
    if (SensorHzTimer >= SensorHzFrequency)
    {
        SensorHzTimer = 0.0f;
        ClearContainers = true;
        return true;
    }
    else
    {
        ClearContainers = false;
        return false;
    }
}
 
void ALidar::ChangeLidarParameters(FLidarParameters newLidarParameters)
{
    if (!CanSimulateSensor())
    {
        // We are not currently simulating sensor, we can safely change parameters here
        LidarParameters = newLidarParameters;
        ChangeParameters();
    }
    else
    {
        // Change parameters at the end of the frame
        TempLidarParameters = newLidarParameters;
        LidarParametersChanged = true;
    }
}
 
uint32 ALidar::GetSemanticData(const FHitResult& HitResult) const
{
    auto* HitComponent = HitResult.Component.Get();
 
    if (HitComponent)
    {
        if (!HitComponent->ComponentTags.IsEmpty())
        {
            FName Tag = HitComponent->ComponentTags[0];
            FColor Color = SemanticColors.FindRef(Tag);
            return (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
        }
        else
        {
            return CachedNoneColor;
        }
    }
    else
    {
        return CachedSnowflakeColor;
    }
}
 
bool ALidar::ShootLaser(FHitResult& HitResult, const FVector& EndTrace, const FCollisionQueryParams& TraceParams,
    const FVector& LidarBodyLocation, const int32 Channel, const bool Semantic, const uint32 RGBDefault)
{
#ifdef ParallelLineTraceSingleByChannel_EXISTS
    // Defined and implemented in our AGRARSENSE Unreal Engine fork
    // This LineTrace method doesn't block the physics scene which improves linetrace performance.
    World->ParallelLineTraceSingleByChannel(
        HitResult,
        LidarBodyLocation,
        EndTrace,
        ECC_GameTraceChannel3,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#else
    // If not using our fork of the engine, use default LineTrace method.
    World->LineTraceSingleByChannel(
        HitResult,
        LidarBodyLocation,
        EndTrace,
        ECC_GameTraceChannel3,
        TraceParams,
        FCollisionResponseParams::DefaultResponseParam);
#endif
 
    // Scale the pointcloud
    HitResult.ImpactPoint *= 1e-2;
 
    // Flip Y axis
    HitResult.ImpactPoint.Y *= -1;
 
    if (NeedToSavePointCloudWithoutNoise)
    {
        // Save Original FHitResult to separate array
        FHitResult OriginalHitResultCopy = HitResult;
        uint32 RGB = RGBDefault;
        if (Semantic)
        {
            // If Lidar is set to Semantic, set point color
            RGB = GetSemanticData(OriginalHitResultCopy);
        }
        const FVector& HitPoint = OriginalHitResultCopy.ImpactPoint;
        FPointData point = { HitPoint.X, HitPoint.Y, HitPoint.Z, RGB };
        PreProcessedHitImpactPoints[Channel].emplace_back(point);
    }
 
    bool BlockingHit = false;
    if (LidarParameters.UseLidarNoiseModel)
    {
        // Check whether we "hit" snowflake by using LUAS formula
        bool HitSnowFlake = LidarNoiseModel::CheckSnowflakeHit(HitResult, EndTrace, LidarBodyLocation, CurrentWeatherParameters);
        BlockingHit = HitResult.bBlockingHit || HitSnowFlake;
    }
    else
    {
        BlockingHit = HitResult.IsValidBlockingHit();
    }
 
    return BlockingHit;
}
 
void ALidar::ResetRecordedHits(int32 MaxPointsPerChannel)
{
    if (!ClearContainers)
    {
        return;
    }
 
    // Before resetting visualize Lidar particles
    VisualizeLidarParticles();
 
    if (!SendDataEveryFrame)
    {
        // If we only send data at Lidar sensor rotation frequency
        // reserve enough points to fill all points
        MaxPointsPerChannel = LidarParameters.PointsPerSecond / LidarParameters.Channels;
    }
 
    if (LidarParameters.SavePointcloudWithoutNoiseModel)
    {
        for (auto& impactPoints : PreProcessedHitImpactPoints)
        {
            impactPoints.clear();
            impactPoints.reserve(MaxPointsPerChannel);
        }
    }
 
    PointsFlattened.clear();
 
    for (auto& impactPoints : Points)
    {
        impactPoints.clear();
        impactPoints.reserve(MaxPointsPerChannel);
    }
}
 
void ALidar::SendDataToTopic(const std::vector<FPointData>& points)
{
    UTopic* Topic = GetROSTopic();
 
    if (!IsROSConnected()
        || !Topic
        || !SendDataToROS
        || !PointCloudMessage.IsValid()
        || points.empty())
    {
        return;
    }
 
    const int pointSize = points.size();
    size_t dataSize = pointSize * sizeof(FPointData);
 
    PointCloudMessage->width = pointSize;
    PointCloudMessage->row_step = dataSize;
    PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
    Topic->Publish(PointCloudMessage);
}
 
void ALidar::SaveDataToDisk()
{
    if (!LidarParameters.SaveDataToDisk)
    {
        return;
    }
 
    // Save point cloud to disk
    FString Filename = FString::Printf(TEXT("%sLidar_%d.ply"), *FileSavePath, LidarSaves);
    UPointcloudUtilities::SaveVectorArrayAsPlyAsync(Filename, PointsFlattened);
 
    if (NeedToSavePointCloudWithoutNoise)
    {
        // Flatten 2D std::vector to 1D std::vector
        const int32 ChannelCount = LidarParameters.Channels;
        std::vector<FPointData> PointsWithOutNoiseModel;
        size_t totalSize = 0;
 
        for (int32 i = 0; i < ChannelCount; ++i)
        {
            totalSize += PreProcessedHitImpactPoints[i].size();
        }
        PointsWithOutNoiseModel.reserve(totalSize);
 
        for (int32 i = 0; i < ChannelCount; ++i)
        {
            PointsWithOutNoiseModel.insert(PointsWithOutNoiseModel.end(), PreProcessedHitImpactPoints[i].begin(), PreProcessedHitImpactPoints[i].end());
        }
 
        // Save pointcloud to disk without any noise
        FString PreprocessedFilename = FString::Printf(TEXT("%sLidar_%d_without_noise_model.ply"), *FileSavePath, LidarSaves);
        UPointcloudUtilities::SaveVectorArrayAsPlyAsync(PreprocessedFilename, PointsWithOutNoiseModel);
    }
 
    ++LidarSaves;
}