Camera.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 "Camera.h"
 
#include "Agrarsense/Sensor/Lidar/LidarManager.h"
#include "Agrarsense/Sensor/Lidar/Lidar.h"
#include "Agrarsense/Widgets/CameraWidgetBase.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Utils/CoordinateConversionUtilities.h"
#include "Agrarsense/Settings/AgrarsenseSettings.h"
#include "Agrarsense/Vehicle/PIDDrone.h"
 
#include "NiagaraComponent.h"
#include "Components/PrimitiveComponent.h"
#include "Materials/Material.h"
#include "ImageUtils.h"
#include "Async/Async.h"
#include "HighResScreenshot.h"
#include "ImageWriteTask.h"
#include "ImageWriteQueue.h"
#include "Misc/DateTime.h"
#include "Math/Color.h"
#include "Blueprint/UserWidget.h"
#include "GameFramework/PlayerController.h"
#include "GeoReferencingSystem.h"
#include "TimerManager.h"
#include "GameFramework/SpringArmComponent.h"
 
ACamera::ACamera(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    PrimaryActorTick.bCanEverTick = true;
    PrimaryActorTick.TickGroup = TG_PostUpdateWork;
 
    CaptureRenderTarget = CreateDefaultSubobject<UTextureRenderTarget2D>(FName(*FString::Printf(TEXT("CaptureRenderTarget"))));
    CaptureRenderTarget->CompressionSettings = TextureCompressionSettings::TC_Default;
    CaptureRenderTarget->SRGB = false;
    //CaptureRenderTarget->bForceLinearGamma = true;
    CaptureRenderTarget->bAutoGenerateMips = false;
    CaptureRenderTarget->bGPUSharedFlag = true;
    //CaptureRenderTarget->RenderTargetFormat = RTF_RGBA8;
    CaptureRenderTarget->AddressX = TextureAddress::TA_Clamp;
    CaptureRenderTarget->AddressY = TextureAddress::TA_Clamp;
 
    CaptureComponent2D = CreateDefaultSubobject<USceneCaptureComponent2D>(FName(*FString::Printf(TEXT("SceneCaptureComponent2D"))));
    CaptureComponent2D->SetupAttachment(RootComponent);
    CaptureComponent2D->PrimitiveRenderMode = ESceneCapturePrimitiveRenderMode::PRM_RenderScenePrimitives;
    CaptureComponent2D->bCaptureOnMovement = false;
    CaptureComponent2D->bCaptureEveryFrame = false;
    CaptureComponent2D->bAlwaysPersistRenderingState = true;
}
 
void ACamera::ChangeCameraParameters(FCameraBaseParameters newParameters)
{
    SetupCamera(newParameters);
}
 
void ACamera::AddPostProcessingMaterial(const FString& Path, float Weight)
{
    UMaterial* PostProcessingMat = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
    if (PostProcessingMat && CaptureComponent2D)
    {
        FPostProcessSettings& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
        PostProcessSettings.AddBlendable(PostProcessingMat, Weight);
    }
    else
    {
        const FString ErrorMessage = FString::Printf(TEXT("Failed to add post-processing material to sensor: %s"), *GetSensorName());
        SimulatorLog::Log(ErrorMessage);
    }
}
 
void ACamera::RemovePostProcessingMaterial(UMaterial* Material)
{
    if (CaptureComponent2D)
    {
        FPostProcessSettings& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
        PostProcessSettings.RemoveBlendable(Material);
    }
}
 
void ACamera::Init(FCameraBaseParameters parameters, bool SimulateSensor)
{
    CreateDataSavePath();
    SetSimulateSensor(SimulateSensor);
    SetupCamera(parameters);
}
 
void ACamera::CreateLogFile()
{
    if (IsValid(LogFile))
    {
        // File has already been created, return
        return;
    }
 
    FLogFileSettings Settings;
    Settings.FileCreationOptions = FFileCreationOptions::Overwrite;
    Settings.FileWriteOptions = FFileWriteOptions::Queue;
    Settings.QueueLength = MAX_int32; // Only write the log after destroying the sensor
    Settings.KeepFileOpen = false;
    Settings.Timestamp = false;
    Settings.OverrideFilePath = true;
    Settings.FilePath = FileSavePath;
 
    LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
    if (IsValid(LogFile))
    {
        const FString FileName = "camera_metadata";
        LogFile->Create(FileName, Settings);
 
        // Write camera metadata first line to explain the values.
        GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
        if (GeoReferencingSystem)
        {
            WriteToLogFile("timestamp, image_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation, GPS latitude, GPS longitude, GPS altitude");
        }
        else
        {
            WriteToLogFile("timestamp, image_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation");
        }
    }
}
 
void ACamera::SetupCamera(FCameraBaseParameters parameters)
{
    if (!CaptureComponent2D)
    {
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: CaptureComponent2D is nullptr!"));
#endif
        return;
    }
 
    CameraParameters = parameters;
 
    SendDataToROS = CameraParameters.SendDataToROS;
 
    if (CameraParameters.TargetFrameRate != 0.0f)
    {
        CameraFrameRate = 1.0f / CameraParameters.TargetFrameRate;
    }
 
    const bool UsePostProcessingEffects = CameraParameters.PostProcessingEffects;
    auto& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
 
    if (parameters.UsePhysicLensDistortionEffect)
    {
        if (!PhysicLensDistortion.IsValid())
        {
            const FString Path = "/Game/Agrarsense/Materials/PostProcessingMaterials/PhysicLensDistortion.PhysicLensDistortion";
            PhysicLensDistortion = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
 
            if (PhysicLensDistortion.IsValid())
            {
                PostProcessSettings.AddBlendable(PhysicLensDistortion.Get(), 1.0f);
#if WITH_EDITOR
                UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Added physics lens distortion effect."));
#endif
            }
        }
    }
    else if (PhysicLensDistortion.IsValid())
    {
        PostProcessSettings.RemoveBlendable(PhysicLensDistortion.Get());
        PhysicLensDistortion.Reset();
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Removed physics lens distortion effect."));
#endif
    }
 
    if (parameters.UseIceLensEffect)
    {
        if (!IceMaterialInstance.IsValid())
        {
            const FString Path = "/Game/Agrarsense/Materials/PostProcessingMaterials/CameraPostProcessEffects/m_ice_lens_effect";
            UMaterial* LoadedIceMaterial = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
 
            // Create UMaterialInstanceDynamic from LoadedIceMaterial
            IceMaterialInstance = UMaterialInstanceDynamic::Create(LoadedIceMaterial, nullptr);
            if (IceMaterialInstance.IsValid())
            {
                // Add material to CaptureComponent2D PostProcessSettings 
                PostProcessSettings.AddBlendable(IceMaterialInstance.Get(), 1.0f);
#if WITH_EDITOR
                UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Added ice lens effect."));
#endif
            }
        }
 
        UMaterialInstanceDynamic* IceMatInstance = IceMaterialInstance.Get();
        if (IceMatInstance)
        {
            // Set IceMaterialInstance scalar parameters values
            IceMatInstance->SetScalarParameterValue(FName("Strength"), parameters.IceLensEffectStrength);
            IceMatInstance->SetScalarParameterValue(FName("Angle"), parameters.IceLensEffectAngle);
#if WITH_EDITOR
            UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Changed ice lens effect parameters."));
#endif
        }
    }
    else if (IceMaterialInstance.IsValid())
    {
        UMaterialInstanceDynamic* IceMatInstance = IceMaterialInstance.Get();
        if (IceMatInstance)
        {
            // Remove material from CaptureComponent2D PostProcessSettings
            PostProcessSettings.RemoveBlendable(IceMatInstance);
            IceMaterialInstance.Reset();
 
#if WITH_EDITOR
            UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Removed ice lens effect."));
#endif
        }
    }
 
    ImageWidth = CameraParameters.Width;
    ImageHeight = CameraParameters.Height;
    const float TargetGamma = CameraParameters.TargetGamma;
    const bool Enable16BitFormat = CameraParameters.Enable16BitFormat;
    bool ForceLinearGamma = !CameraParameters.PostProcessingEffects;
 
    ESensorTypes SensorType = GetSensorType();
    EPixelFormat PixelFormat = Enable16BitFormat ? PF_FloatRGB : PF_B8G8R8A8;
 
    if (SensorType == ESensorTypes::InstanceSegmentationCamera)
    {
        PixelFormat = PF_B8G8R8A8;
        ForceLinearGamma = true;
    }
 
    CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, PixelFormat, ForceLinearGamma);
    //CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, PF_B8G8R8A8, true);
    CaptureRenderTarget->TargetGamma = TargetGamma;
    CaptureRenderTarget->UpdateResourceImmediate(true);
 
    // Set PostProcessSettings override (on or off)
    PostProcessSettings.bOverride_AutoExposureMethod = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AutoExposureBias = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AutoExposureMinBrightness = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AutoExposureMaxBrightness = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AutoExposureSpeedUp = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AutoExposureSpeedDown = UsePostProcessingEffects;
    PostProcessSettings.bOverride_HistogramLogMin = UsePostProcessingEffects;
    PostProcessSettings.bOverride_HistogramLogMax = UsePostProcessingEffects;
    PostProcessSettings.bOverride_CameraShutterSpeed = UsePostProcessingEffects;
    PostProcessSettings.bOverride_CameraISO = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldFstop = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldMinFstop = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldBladeCount = UsePostProcessingEffects;
    PostProcessSettings.bOverride_FilmSlope = UsePostProcessingEffects;
    PostProcessSettings.bOverride_FilmToe = UsePostProcessingEffects;
    PostProcessSettings.bOverride_FilmShoulder = UsePostProcessingEffects;
    PostProcessSettings.bOverride_FilmWhiteClip = UsePostProcessingEffects;
    PostProcessSettings.bOverride_FilmBlackClip = UsePostProcessingEffects;
    PostProcessSettings.bOverride_MotionBlurAmount = UsePostProcessingEffects;
    PostProcessSettings.bOverride_MotionBlurMax = UsePostProcessingEffects;
    PostProcessSettings.bOverride_MotionBlurPerObjectSize = UsePostProcessingEffects;
    PostProcessSettings.bOverride_WhiteTemp = UsePostProcessingEffects;
    PostProcessSettings.bOverride_WhiteTint = UsePostProcessingEffects;
    PostProcessSettings.bOverride_ColorContrast = UsePostProcessingEffects;
    PostProcessSettings.bOverride_SceneFringeIntensity = UsePostProcessingEffects;
    PostProcessSettings.bOverride_ChromaticAberrationStartOffset = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionIntensity = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionRadius = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionStaticFraction = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionFadeDistance = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionPower = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionBias = UsePostProcessingEffects;
    PostProcessSettings.bOverride_AmbientOcclusionQuality = UsePostProcessingEffects;
    PostProcessSettings.bOverride_BloomMethod = UsePostProcessingEffects;
    PostProcessSettings.bOverride_BloomIntensity = UsePostProcessingEffects;
    PostProcessSettings.bOverride_BloomThreshold = UsePostProcessingEffects;
    PostProcessSettings.bOverride_LensFlareIntensity = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldFocalDistance = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldDepthBlurAmount = UsePostProcessingEffects;
    PostProcessSettings.bOverride_DepthOfFieldDepthBlurRadius = UsePostProcessingEffects;
 
    // Set PostProcessSettings values
    PostProcessSettings.CameraShutterSpeed = CameraParameters.ShutterSpeed;
    PostProcessSettings.CameraISO = CameraParameters.ISO;
    PostProcessSettings.DepthOfFieldFstop = CameraParameters.Aperture;
    PostProcessSettings.DepthOfFieldFocalDistance = CameraParameters.FocalDistance;
    PostProcessSettings.DepthOfFieldDepthBlurAmount = CameraParameters.DepthBlurAmount;
    PostProcessSettings.DepthOfFieldDepthBlurRadius = CameraParameters.DepthBlurRadius;
    PostProcessSettings.DepthOfFieldMinFstop = CameraParameters.DofMinFStop;
    PostProcessSettings.DepthOfFieldBladeCount = CameraParameters.DofBladeCount;
    PostProcessSettings.FilmSlope = CameraParameters.FilmSlope;
    PostProcessSettings.FilmToe = CameraParameters.FilmToe;
    PostProcessSettings.FilmShoulder = CameraParameters.FilmShoulder;
    PostProcessSettings.FilmBlackClip = CameraParameters.FilmBlackClip;
    PostProcessSettings.FilmWhiteClip = CameraParameters.FilmWhiteClip;
    PostProcessSettings.AutoExposureMinBrightness = CameraParameters.ExposureMinBrightness;
    PostProcessSettings.AutoExposureMaxBrightness = CameraParameters.ExposureMaxBrightness;
    PostProcessSettings.AutoExposureSpeedUp = CameraParameters.ExposureSpeedUp;
    PostProcessSettings.AutoExposureSpeedDown = CameraParameters.ExposureSpeedDown;
    PostProcessSettings.MotionBlurAmount = CameraParameters.MotionBlurIntensity;
    PostProcessSettings.MotionBlurMax = CameraParameters.MotionBlurMax;
    PostProcessSettings.MotionBlurPerObjectSize = CameraParameters.MotionBlurMinObjSize;
    PostProcessSettings.LensFlareIntensity = CameraParameters.LensFlareIntensity;
    PostProcessSettings.BloomIntensity = CameraParameters.BloomIntensity;
    PostProcessSettings.WhiteTemp = CameraParameters.WhiteTemp;
    PostProcessSettings.WhiteTint = CameraParameters.WhiteTint;
    PostProcessSettings.SceneFringeIntensity = CameraParameters.ChromAberrIntensity;
    PostProcessSettings.ChromaticAberrationStartOffset = CameraParameters.ChromAberrOffset;
 
    CaptureComponent2D->FOVAngle = CameraParameters.FOV;
    CaptureComponent2D->TextureTarget = CaptureRenderTarget;
 
    if (CameraParameters.MaxViewDistanceInCmOverride > 0)
    {
        CaptureComponent2D->MaxViewDistanceOverride = CameraParameters.MaxViewDistanceInCmOverride;
    }
    else
    {
        CaptureComponent2D->MaxViewDistanceOverride = -1.0f; // Infinite
    }
 
    if (CameraParameters.UseHDR && GetSensorType() == ESensorTypes::RGBCamera)
    {
        // Enable HDR on CaptureSource, only allowed for RGB Camera for now.
        CaptureComponent2D->CaptureSource = ESceneCaptureSource::SCS_FinalToneCurveHDR;
    }
    else
    {
        CaptureComponent2D->CaptureSource = ESceneCaptureSource::SCS_FinalColorLDR;
    }
 
    CaptureComponent2D->bUseRayTracingIfEnabled = true;
    CaptureComponent2D->UpdateContent();
    CaptureComponent2D->Activate();
 
    SetTemporalAA(CameraParameters.UseTemporalAA);
 
    const FString Encoding = "bgr8";
 
    // Create ROS Image message
    ImageMsg.Reset();
    ImageMsg = MakeShared<ROSMessages::sensor_msgs::Image>();
    ImageMsg->header.frame_id = "world";
    ImageMsg->height = ImageWidth;
    ImageMsg->width = ImageHeight;
    ImageMsg->encoding = Encoding;
    ImageMsg->is_bigendian = 0;
    ImageMsg->step = ImageWidth * 4;
 
    CreateROSTopic();
 
    if (!UnrealWindow.IsValid())
    {
        // Create and setup UnrealWindow
        const FString WindowName = GetSensorIdentifier() + " Window";
        UnrealWindow = MakeShareable(new FUnrealWindow(ImageWidth, ImageHeight, WindowName, UAgrarsenseSettings::ShouldStartWindowMinimized()));
 
        // Set on Window closed event
        OnWindowClosedDelegate.BindUObject(this, &ACamera::OnWindowClosed);
        UnrealWindow->GetSWindow()->SetOnWindowClosed(OnWindowClosedDelegate);
 
        // try to create Camera parameter Widget from path
        FSoftClassPath WidgetClassPath(TEXT("/Game/Agrarsense/Widgets/Camera/WBP_CameraControlsMenu.WBP_CameraControlsMenu_C"));
        UClass* WidgetClass = WidgetClassPath.TryLoadClass<UUserWidget>();
 
        APlayerController* PlayerController = UGameplayStatics::GetPlayerController(GetWorld(), 0);
 
        if (WidgetClass && PlayerController)
        {
            UCameraWidgetBase* WidgetInstance = CreateWidget<UCameraWidgetBase>(PlayerController, WidgetClass);
            if (WidgetInstance)
            {
                bool ShowGuide = false;
                WidgetInstance->Setup(this, ShowGuide);
                this->AddWidgetToWindow(WidgetInstance);
            }
        }
    }
 
    bool Resized = false;
    if (UnrealWindow.IsValid())
    {
        UnrealWindow->SetupComponent(CaptureRenderTarget);
        if (UnrealWindow->GetWindowWidth() != ImageWidth || UnrealWindow->GetWindowHeight() != ImageHeight)
        {
            UnrealWindow->ResizeWindow(ImageWidth, ImageHeight);
            Resized = true;
        }
    }
 
    StartFrameGrabber();
 
    if (Resized)
    {
        OnCameraWindowResized.Broadcast(this, CameraParameters);
    }
 
    if (CameraParameters.SaveImageToDisk)
    {
        CreateLogFile();
    }
}
 
void ACamera::BeginPlay()
{
    Super::BeginPlay();
 
    // Register to PreActorTick where USceneCaptureComponent2D will get rendered.
    OnPreTickDelegate = FWorldDelegates::OnWorldPreActorTick.AddUObject(this, &ACamera::PreActorTick);
 
    // Check if we should use ATickManager which ticks in parallel, or use Post actor tick
    // In there we capture the screen pixels with FrameGrabber API, 
    // might do some extra processing, save image to disk and send data to ROS. 
    if (UseParallelLateTick)
    {
        SetActorTickEnabled(false);
        TickEntry = ATickManager::AddTick(this, BindTick(this, &ACamera::EndOfFrameParellel), ETickType::LateTickParallel);
    }
    else
    {
        SetActorTickEnabled(true);
 
        if (UseReadPixels) 
        {
            UE_LOG(LogTemp, Warning, TEXT("juu"));
            OnPostTickDelegate = FWorldDelegates::OnWorldPostActorTick.AddUObject(this, &ACamera::PixelReadEndOfFrame);
        }
        else 
        {
            OnPostTickDelegate = FWorldDelegates::OnWorldPostActorTick.AddUObject(this, &ACamera::EndOfFrame);
        }
    }
 
    // Get all existing components set to be hidden for all Camera sensors (like Lidar point cloud visualization).
    auto Components = ASensor::GetComponentsToHide();
    for (int32 i = 0; i < Components.Num(); i++)
    {
        UPrimitiveComponent* Primitive = Components[i].Get();
        if (Primitive)
        {
            HidePrimitiveComponent(Primitive);
        }
    }
 
    OnPrimitiveAdded.AddUniqueDynamic(this, &ACamera::HidePrimitiveComponent);
 
    GetWorld()->GetTimerManager().SetTimerForNextTick([this]()
        {
            if (IsValid(this))
            {
                DronePtr = Cast<APIDDrone>(GetAttachParentActor());
                if (!DronePtr)
                {
                    AllowGimbal = false;
                }
            }
        });
}
 
void ACamera::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    FWorldDelegates::OnWorldPreActorTick.Remove(OnPreTickDelegate);
 
    if (UseParallelLateTick)
    {
        ATickManager::RemoveTick(TickEntry);
    }
    else
    {
        FWorldDelegates::OnWorldPostActorTick.Remove(OnPostTickDelegate);
    }
 
    if (UnrealWindow.IsValid())
    {
        OnWindowClosedDelegate.Unbind();
        UnrealWindow->DestroyWindow();
        UnrealWindow.Reset();
    }
 
    ReleaseFrameGrabber();
 
    if (CaptureRenderTarget)
    {
        CaptureRenderTarget->ConditionalBeginDestroy();
        CaptureRenderTarget = nullptr;
    }
 
    if (CaptureComponent2D)
    {
        CaptureComponent2D->UnregisterComponent();
        CaptureComponent2D->ConditionalBeginDestroy();
        CaptureComponent2D = nullptr;
    }
 
    ImageMsg.Reset();
    IceMaterialInstance.Reset();
 
    if (BGR8Buffer)
    {
        delete[] BGR8Buffer;
        BGR8Buffer = nullptr;
    }
    CurrentBufferSize = 0;
}
 
void ACamera::PreActorTick(UWorld* World, ELevelTick TickType, float DeltaSeconds)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::PreActorTick);
 
    UpdateCameraStabilization();
 
    if (ShouldSimulate(DeltaSeconds) && CaptureComponent2D)
    {
        // Capture scene now.
        // We could use CaptureSceneDeferred but it would not work if World (Spectator) Rendering is disabled.
        CaptureComponent2D->CaptureScene();
 
        // Inform FrameGrabber to capture this frame
        if (FrameGrabber.IsValid() && !UseReadPixels)
        {
            FrameGrabber->CaptureThisFrame(FFramePayloadPtr());
        }
    }
}
 
void ACamera::UpdateCameraStabilization()
{
    // If this Camera sensor is attached to Drone, 
    // keep the camera steady kind of like a Gimbal.
    if (AllowGimbal && DronePtr && !DronePtr->IsVehicleInGarage())
    {
        if (SensorSetRotation == FRotator::ZeroRotator)
        {
            // Save garage sensor values
            SensorSetRotation = GetTransform().Rotator();
        }
        FRotator GimbalRot = FRotator(SensorSetRotation.Pitch, GetActorRotation().Yaw, SensorSetRotation.Roll);
        SetActorRotation(GimbalRot);
    }
}
 
void ACamera::EndOfFrame(UWorld* World, ELevelTick TickType, float DeltaSeconds)
{
    FrameGrabberCapture();
}
 
void ACamera::PixelReadEndOfFrame(UWorld* World, ELevelTick TickType, float DeltaSeconds)
{
    ReadPixelsCapture();
}
 
void ACamera::EndOfFrameParellel(float DeltaTime)
{
    FrameGrabberCapture();
}
 
bool ACamera::ShouldSimulate(const float DeltaSeconds)
{
    if (!CanSimulateSensor())
    {
        ShouldSimulateCamera = false;
        return ShouldSimulateCamera;
    }
 
    if (CameraParameters.TargetFrameRate == 0.0f)
    {
        ShouldSimulateCamera = true;
        return ShouldSimulateCamera;
    }
 
    FrameRateTimer += DeltaSeconds;
    if (FrameRateTimer >= CameraFrameRate)
    {
        FrameRateTimer = 0.0f;
        ShouldSimulateCamera = true;
        return ShouldSimulateCamera;
    }
 
    ShouldSimulateCamera = false;
 
    return ShouldSimulateCamera;
}
 
void ACamera::FrameGrabberCapture()
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::FrameGrabberCapture);
 
    if (!ShouldSimulateCamera || !FrameGrabber.IsValid())
    {
        return;
    }
 
    if (!SendDataToROS && !CameraParameters.SaveImageToDisk && !SaveCurrentFrameToDiskRequested)
    {
        return;
    }
 
    TArray<FCapturedFrameData> Frames = FrameGrabber->GetCapturedFrames();
    if (Frames.Num())
    {
        FCapturedFrameData& LastFrame = Frames.Last();
        TArray<FColor>& ImageBuffer = LastFrame.ColorBuffer;
 
        AddProcessingToFrameBuffer(ImageBuffer);
 
        if (CameraParameters.SaveImageToDisk || SaveCurrentFrameToDiskRequested)
        {
            if (SaveCurrentFrameToDiskRequested)
            {
                CreateLogFile();
                SaveCurrentFrameToDiskRequested = false;
            }
 
            AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [this, ImageBuffer]()
                {
                    SaveImageToDisk(ImageBuffer, ImageWidth, ImageHeight);
                });
        }
 
        SendImageDataToROS(ImageBuffer, ImageWidth, ImageHeight);
    }
}
 
void ACamera::ReadPixelsCapture()
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::ReadPixelsCapture);
 
    if (!ShouldSimulateCamera)
    {
        return;
    }
 
    if (!SendDataToROS && !CameraParameters.SaveImageToDisk && !SaveCurrentFrameToDiskRequested)
    {
        return;
    }
 
    TArray<FColor> ImageBuffer;
    ImageBuffer.Reserve(ImageWidth * ImageHeight);
    FReadSurfaceDataFlags Flags(RCM_UNorm);
    Flags.SetLinearToGamma(false); // IMPORTANT to get instance segmentation ID's correctly
    auto* Resource = CaptureRenderTarget->GameThread_GetRenderTargetResource();
    const bool succeeded = Resource->ReadPixels(ImageBuffer, Flags);
 
    if (CameraParameters.SaveImageToDisk || SaveCurrentFrameToDiskRequested)
    {
        if (SaveCurrentFrameToDiskRequested)
        {
            CreateLogFile();
            SaveCurrentFrameToDiskRequested = false;
        }
 
        AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [this, ImageBuffer]()
            {
                SaveImageToDisk(ImageBuffer, ImageWidth, ImageHeight);
            });
    }
 
    SendImageDataToROS(ImageBuffer, ImageWidth, ImageHeight);
}
 
void ACamera::AddProcessingToFrameBuffer(TArray<FColor>& buffer)
{
    return;
}
 
void ACamera::ApplyGammaCorrectionLUT(TArray<FColor>& Buffer)
{
    static uint8 GammaLUT[256];
    static bool LUTInitialized = false;
    if (!LUTInitialized)
    {
        LUTInitialized = true;
        for (int32 i = 0; i < 256; ++i)
        {
            float Linear = FMath::Pow(i / 255.0f, 2.2f);
            GammaLUT[i] = FMath::RoundToInt(Linear * 255.0f);
        }
    }
 
    FColor* RESTRICT BufferPtr = Buffer.GetData();
    const int32 Num = Buffer.Num();
 
    const int32 ChunkSize = 512;
    const int32 NumChunks = (Num + ChunkSize - 1) / ChunkSize;
 
    ParallelFor(NumChunks, [BufferPtr, Num](int32 ChunkIndex)
        {
            const int32 Start = ChunkIndex * ChunkSize;
            const int32 End = FMath::Min(Start + ChunkSize, Num);
 
            for (int32 i = Start; i < End; ++i)
            {
                FColor& C = BufferPtr[i];
                C.R = GammaLUT[C.R];
                C.G = GammaLUT[C.G];
                C.B = GammaLUT[C.B];
            }
        }, false);
}
 
void ACamera::HidePrimitiveComponent(UPrimitiveComponent* PrimitiveComponent)
{
    if (PrimitiveComponent && CaptureComponent2D)
    {
        CaptureComponent2D->HideComponent(PrimitiveComponent);
    }
}
 
void ACamera::StartFrameGrabber()
{
    if (!UnrealWindow.IsValid())
    {
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: UnrealWindow is nullptr!"));
#endif
        return;
    }
 
    // Destroy old FrameGrabber
    ReleaseFrameGrabber();
 
    TSharedPtr<FSceneViewport> SceneViewport = UnrealWindow->GetSceneViewport();
    if (SceneViewport.IsValid() && CaptureRenderTarget)
    {
        EPixelFormat PixelFormat = GetPixelFormatFromRenderTargetFormat(TextureFormat);
        FIntPoint Size = SceneViewport->GetSize();
 
        if (GetSensorType() == ESensorTypes::DVSCamera)
        {
            // Currently only DVS camera needs this texture to render DVS output into separate FUnrealWindow.
            CaptureFrameTexture = UTexture2D::CreateTransient(Size.X, Size.Y, PixelFormat);
            CaptureFrameTexture->UpdateResource();
        }
 
        FrameGrabber = MakeShareable(new FFrameGrabber(SceneViewport.ToSharedRef(), Size, PixelFormat));
        FrameGrabber->StartCapturingFrames();
    }
}
 
void ACamera::ReleaseFrameGrabber()
{
    if (FrameGrabber.IsValid())
    {
        FrameGrabber->StopCapturingFrames();
        FrameGrabber->Shutdown();
        FrameGrabber.Reset();
    }
 
    if (CaptureFrameTexture)
    {
        CaptureFrameTexture->RemoveFromRoot();
        CaptureFrameTexture->ConditionalBeginDestroy();
        CaptureFrameTexture = nullptr;
    }
}
 
void ACamera::SendImageDataToROS(const TArray<FColor>& FrameBuffer, int32 Width, int32 Height)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::SendImageDataToROS);
 
    UTopic* Topic = GetROSTopic();
    if (!SendDataToROS
        || !Topic
        || !IsROSConnected()
        || FrameBuffer.IsEmpty()
        || !ImageMsg.IsValid())
    {
        return;
    }
 
    // Cast TArray<FColor> into uint8[] data with all channels (RBGA)
    // If you want to send RGBA data, uncomment the following lines
    // and comment the BGR8Buffer conversion below
    //ImageMsg->encoding = "bgra8";
    //ImageMsg->step = Width * 4;
    //ImageMsg->data = const_cast<uint8*>(reinterpret_cast<const uint8*>(FrameBuffer.GetData()));
 
    // Convert TArray<FColor> to uint8[] data without alpha channel
    // By removing alpha channel, we reduce ~25% of ROS bandwidth, 
    // but costs a bit more in processing time here
    const int32 BufferSize = Width * Height * 3;
    if (!BGR8Buffer || CurrentBufferSize != BufferSize)
    {
        // Resize buffer if needed
        delete[] BGR8Buffer;
        BGR8Buffer = new uint8[BufferSize];
        CurrentBufferSize = BufferSize;
    }
 
    const FColor* RESTRICT Source = FrameBuffer.GetData();
    uint8* RESTRICT Dest = BGR8Buffer;
    const int32 PixelCount = FrameBuffer.Num();
    const int32 ChunkSize = 512;
    const int32 NumChunks = (PixelCount + ChunkSize - 1) / ChunkSize;
 
    ParallelFor(NumChunks, [Source, Dest, ChunkSize, PixelCount](int32 ChunkIndex)
        {
            const int32 StartIndex = ChunkIndex * ChunkSize;
            const int32 EndIndex = FMath::Min(StartIndex + ChunkSize, PixelCount);
 
            for (int32 i = StartIndex; i < EndIndex; ++i)
            {
                const FColor& Pixel = Source[i];
                const int32 j = i * 3;
                Dest[j + 0] = Pixel.B;
                Dest[j + 1] = Pixel.G;
                Dest[j + 2] = Pixel.R;
            }
        }, false);
 
    ImageMsg->data = BGR8Buffer;
    ImageMsg->step = Width * 3;
 
    ImageMsg->height = Height;
    ImageMsg->width = Width;
    ImageMsg->header.time = FROSTime::Now();
 
    Topic->Publish(ImageMsg);
}
 
void ACamera::SaveImageToDisk(const TArray<FColor> FrameBuffer, int32 Width, int32 Height)
{
    TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::SaveImageToDisk);
 
    if (FrameBuffer.IsEmpty() || Width == 0 || Height == 0)
    {
        return;
    }
 
    int32 Size = Width * Height;
    if (FrameBuffer.Num() != Size)
    {
        const FString Msg = "Camera sensor: Unable to save Image to disk. FrameBuffer size and resolution don't match!";
        SimulatorLog::Log(Msg);
        return;
    }
 
    const FString ImageName = FString::Printf(TEXT("image_%d.png"), ImageNumber);
    const FString FullFileName = FString::Printf(TEXT("%s%s"), *FileSavePath, *ImageName);
    ++ImageNumber;
 
    SaveCameraMetaDataToDisk(ImageName);
 
    FIntPoint DestSize(Width, Height);
    TImagePixelData<FColor> PixelData(DestSize);
    PixelData.Pixels = FrameBuffer;
 
    // Create ImageTask
    TUniquePtr<FImageWriteTask> ImageTask = MakeUnique<FImageWriteTask>();
    ImageTask->PixelData = MakeUnique<TImagePixelData<FColor>>(PixelData);
    ImageTask->Filename = FullFileName;
    ImageTask->Format = EImageFormat::PNG;
    ImageTask->CompressionQuality = (int32)EImageCompressionQuality::Default;
    ImageTask->bOverwriteFile = true;
    ImageTask->PixelPreProcessors.Add(TAsyncAlphaWrite<FColor>(255));
 
    // Get Screenshot config and enqueue image save task
    FHighResScreenshotConfig& HighResScreenshotConfig = GetHighResScreenshotConfig();
    TFuture<bool> CompletionFuture = HighResScreenshotConfig.ImageWriteQueue->Enqueue(MoveTemp(ImageTask));
}
 
void ACamera::SaveCameraMetaDataToDisk(const FString& ImageName)
{
    if (!IsValid(LogFile))
    {
        // If the log file located in base class is not valid, return here.
        return;
    }
 
    const FVector ActorPosition = GetActorLocation();
    const FRotator ActorRotation = GetActorRotation();
 
    const FString TimeStamp = CreateTimeStampString();
    FString MetaData;
    
    if (GeoReferencingSystem)
    {
        FGeographicCoordinates GeoCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
        MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.8f, %.8f, %.8f"),
            *TimeStamp, *ImageName,
            ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll,
            GeoCoordinates.Latitude, GeoCoordinates.Longitude, GeoCoordinates.Altitude);
    }
    else
    {
        MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f"),
            *TimeStamp, *ImageName,
            ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
            ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll);
    }
 
    // Write to the log file (this is written after sensor is destroyed)
    WriteToLogFile(MetaData);
}
 
void ACamera::OnWindowClosed(const TSharedRef<SWindow>& Window)
{
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Camera window closed. Destroying Camera sensor.."));
#endif
 
    // Broadcast event that this Camera sensor is about be destroyed when user has clicked Window Close button.
    OnCameraWindowClosed.Broadcast(this);
 
    // Destroy this camera sensor
    this->Destroy();
}
 
void ACamera::AddWidgetToWindow(UWidget* WidgetToAdd)
{
    if (UnrealWindow.IsValid() && WidgetToAdd)
    {
        UnrealWindow->AddUWidgetToWindow(WidgetToAdd);
    }
}
 
void ACamera::RemoveWidgetFromWindow(UWidget* WidgetToRemove)
{
    if (UnrealWindow.IsValid() && WidgetToRemove)
    {
        UnrealWindow->RemoveUWidgetFromWindow(WidgetToRemove);
    }
}
 
void ACamera::SetShadowRendering(bool RenderShadows)
{
    if (CaptureComponent2D)
    {
        auto& CameraShowFlags = CaptureComponent2D->ShowFlags;
        CameraShowFlags.SetDynamicShadows(RenderShadows);
        CameraShowFlags.SetContactShadows(RenderShadows);
        CameraShowFlags.SetCapsuleShadows(RenderShadows);
    }
}
 
void ACamera::SetTemporalAA(bool SetTemporal)
{
    if (CaptureComponent2D)
    {
        auto& CameraShowFlags = CaptureComponent2D->ShowFlags;
        CameraShowFlags.SetTemporalAA(SetTemporal);
    }
}
 
void ACamera::ResizeCamera(int32 Width, int32 Height)
{
    if (Width == 0 || Height == 0)
    {
        return;
    }
 
    CameraParameters.Width = Width;
    CameraParameters.Height = Height;
    ChangeCameraParameters(CameraParameters);
}
 
void ACamera::AddHiddenActor(AActor* Actor)
{
    if (Actor && CaptureComponent2D)
    {
        CaptureComponent2D->HiddenActors.AddUnique(Actor);
    }
}
 
void ACamera::DisableShowFlags()
{
    if (!CaptureComponent2D)
    {
        return;
    }
 
    auto& ShowFlags = CaptureComponent2D->ShowFlags;
 
    ShowFlags.SetDeferredLighting(false);
    ShowFlags.SetSkyLighting(false);
    ShowFlags.SetAmbientOcclusion(false);
    ShowFlags.SetAntiAliasing(false);
    ShowFlags.SetVolumetricFog(false);;
    ShowFlags.SetBloom(false);
    ShowFlags.SetCameraImperfections(false);
    ShowFlags.SetCameraInterpolation(false);
    ShowFlags.SetColorGrading(false);
    ShowFlags.SetDepthOfField(false);
    ShowFlags.SetDiffuse(false);
    ShowFlags.SetDirectionalLights(false);
    ShowFlags.SetDirectLighting(false);
    ShowFlags.SetDynamicShadows(false);
    ShowFlags.SetEyeAdaptation(false);
    ShowFlags.SetFog(false);
    ShowFlags.SetGlobalIllumination(false);
    ShowFlags.SetGrain(false);
    ShowFlags.SetHLODColoration(false);
    ShowFlags.SetHMDDistortion(false);
    ShowFlags.SetLensFlares(false);
    ShowFlags.SetLightComplexity(false);
    ShowFlags.SetLightFunctions(false);
    ShowFlags.SetLightInfluences(false);
    ShowFlags.SetLighting(false);
    ShowFlags.SetLightMapDensity(false);
    ShowFlags.SetLightRadius(false);
    ShowFlags.SetLightShafts(false);
    ShowFlags.SetLODColoration(false);
    ShowFlags.SetMotionBlur(false);
    ShowFlags.SetOnScreenDebug(false);
    ShowFlags.SetParticles(false);
    ShowFlags.SetPointLights(false);
    ShowFlags.SetRefraction(false);
    ShowFlags.SetSceneColorFringe(false);
    ShowFlags.SetScreenSpaceAO(false);
    ShowFlags.SetScreenSpaceReflections(false);
    ShowFlags.SetSkyLighting(false);
    ShowFlags.SetSpotLights(false);
    ShowFlags.SetStationaryLightOverlap(false);
    ShowFlags.SetSubsurfaceScattering(false);
    ShowFlags.SetTonemapper(false);
    ShowFlags.SetVisualizeBuffer(false);
    ShowFlags.SetVisualizeDistanceFieldAO(false);
    ShowFlags.SetVisualizeDOF(false);
    ShowFlags.SetVisualizeHDR(false);
    ShowFlags.SetVisualizeLightCulling(false);
    ShowFlags.SetVisualizeMeshDistanceFields(false);
    ShowFlags.SetVisualizeMotionBlur(false);
    ShowFlags.SetVisualizeOutOfBoundsPixels(false);
    ShowFlags.SetVisualizeSenses(false);
    ShowFlags.SetVisualizeShadingModels(false);
    ShowFlags.SetVisualizeSSR(false);
    ShowFlags.SetVisualizeSSS(false);
}