DataCapture.cpp

Go to the documentation of this file.

// Copyright (c) 2024 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 "DataCapture.h"
#include "Agrarsense/Sensor/Camera/Camera.h"
#include "Agrarsense/Sensor/Lidar/Lidar.h"
#include "Agrarsense/Utils/CoordinateConversionUtilities.h"
#include "Agrarsense/Logging/SimulatorLog.h"
#include "Agrarsense/Settings/AgrarsenseSettings.h"
#include "Agrarsense/Game/AgrarsenseStatics.h"
 
#include "GeoReferencingSystem.h"
#include "TimerManager.h"
 
ADataCapture::ADataCapture(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    RootComponent = CreateDefaultSubobject<USceneComponent>(TEXT("SceneRoot"));
    PrimaryActorTick.bCanEverTick = true;
    PrimaryActorTick.bStartWithTickEnabled = false;
    PrimaryActorTick.TickGroup = TG_PrePhysics;
    SetActorTickEnabled(false);
}
 
void ADataCapture::SetupDataCapture(FCaptureData NewCaptureData)
{
    // Separate Cameras and Lidars from the Sensors array into their respective arrays
    // after JsonParser has populated the Sensors array.
    for (ASensor* Sensor : NewCaptureData.Sensors)
    {
        if (ACamera* CameraPtr = Cast<ACamera>(Sensor))
        {
            NewCaptureData.Cameras.Add(CameraPtr);
        }
        else if (ALidar* LidarPtr = Cast<ALidar>(Sensor))
        {
            NewCaptureData.Lidars.Add(LidarPtr);
        }
    }
 
    CaptureData = NewCaptureData;
 
    if (CaptureData.UseGPSLocation)
    {
        AGeoReferencingSystem* GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
 
        if (GeoReferencingSystem)
        {
            for (FTransform& CaptureTransform : CaptureData.CapturePositions)
            {
                FVector Location = CaptureTransform.GetLocation();
 
                float StoredZ = Location.Z;
 
                FGeographicCoordinates GeographicCoordinates;
                GeographicCoordinates.Latitude = Location.X;
                GeographicCoordinates.Longitude = Location.Y;
                GeographicCoordinates.Altitude = Location.Z;
 
                FVector InUnrealCoordinates = UCoordinateConversionUtilities::GeographicToUnrealCoordinates(GeoReferencingSystem, GeographicCoordinates);
 
                if (!CaptureData.UseHeightAboveGround) 
                {
                    CaptureTransform.SetLocation(InUnrealCoordinates);
                }
                else 
                {
                    // If user gives wanted Z in meters from the ground, compute the height
                    InUnrealCoordinates.Z = StoredZ;
                    CaptureTransform.SetLocation(InUnrealCoordinates);
                    ComputeAndSetZFromGround(CaptureTransform);
                }
            }
        }
        else
        {
            SimulatorLog::Log("Couldn't convert GPS coordinates into Unreal transform because couldn't find GeoReferencingSystem.");
        }
    }
 
    if (CaptureData.CaptureRotatedViews)
    {
        // Loop through CapturePositions and add 4 additional positions for 360-degree capture
        TArray<FTransform> NewCapturePositions;
 
        for (const FTransform& OriginalTransform : CaptureData.CapturePositions)
        {
            FVector OriginalPosition = OriginalTransform.GetLocation();
            FRotator OriginalRotation = OriginalTransform.GetRotation().Rotator();
            int32 NumPhotos = 4;
 
            NewCapturePositions.Add(OriginalTransform);
 
            for (int32 i = 0; i < NumPhotos; i++)
            {
                FTransform CameraTransform = FTransform::Identity;
 
                switch (i)
                {
                case 0: // Front
                    CameraTransform.SetLocation(OriginalPosition + FVector(0, 100, 0));
                    CameraTransform.SetRotation(FQuat(OriginalRotation));
                    break;
                case 1: // Back
                    CameraTransform.SetLocation(OriginalPosition + FVector(0, -100, 0));
                    CameraTransform.SetRotation(FQuat(OriginalRotation + FRotator(0, 180, 0)));
                    break;
                case 2: // Left
                    CameraTransform.SetLocation(OriginalPosition + FVector(-100, 0, 0));
                    CameraTransform.SetRotation(FQuat(OriginalRotation + FRotator(0, 90, 0)));
                    break;
                case 3: // Right
                    CameraTransform.SetLocation(OriginalPosition + FVector(100, 0, 0));
                    CameraTransform.SetRotation(FQuat(OriginalRotation + FRotator(0, 270, 0)));
                    break;
                }
 
                // Add the new position to NewCapturePositions
                NewCapturePositions.Add(CameraTransform);
            }
        }
 
        CaptureData.CapturePositions = NewCapturePositions;
    }
 
    if (CaptureData.CapturePositions.Num() != 0 && CaptureData.Cameras.Num() != 0)
    {
        for (int32 i = 0; i < CaptureData.Cameras.Num(); i++)
        {
            ACamera* CameraPtr = CaptureData.Cameras[i];
            if (CameraPtr)
            {
                FCameraBaseParameters params = CameraPtr->GetCameraParameters();
                params.SaveImageToDisk = false;
                params.SendDataToROS = false;
                CameraPtr->ChangeCameraParameters(params);
            }
        }
 
        UpdatePositions(CaptureData.CapturePositions[0]);
 
        // Enable This Actor tick with delay
        FTimerHandle Handle;
        GetWorld()->GetTimerManager().SetTimer(Handle, FTimerDelegate::CreateLambda([this]
            {
                UpdatePositions(CaptureData.CapturePositions[0]);
                SetActorTickEnabled(true);
            }), 0.1f, false);
    }
    else
    {
        DestroySensors();
        Destroy();
    }
}
 
void ADataCapture::BeginPlay()
{
    Super::BeginPlay();
}
 
void ADataCapture::Tick(float DeltaTime)
{
    Super::Tick(DeltaTime);
    Capture();
}
 
void ADataCapture::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    CaptureData.Cameras.Empty();
    CaptureData.CapturePositions.Empty();
}
 
void ADataCapture::Capture()
{
    if (CaptureData.CurrentCaptureIndex >= CaptureData.CapturePositions.Num())
    {
        // if this is the last position, we can stop capturing and destroy the sensors,
        // as well as this Actor.
 
        SetActorTickEnabled(false);
 
        // Destroy this Actor with delay so Camera sensors have time to save the last images
        FTimerHandle Handle;
        GetWorld()->GetTimerManager().SetTimer(Handle, FTimerDelegate::CreateLambda([this]
            {
                DestroySensors();
                Destroy();
            }), 1.0f, false);
 
        return;
    }
 
    switch (CaptureStep)
    {
    case ECaptureStep::WaitingForPositionUpdate:
    {
        // Wait one frame before moving actor
        CaptureStep = ECaptureStep::WaitingAfterPositionUpdate;
        FrameCounter = 0;
        break;
    }
    case ECaptureStep::WaitingAfterPositionUpdate:
    {
        UpdatePositions(CaptureData.CapturePositions[CaptureData.CurrentCaptureIndex]);
 
        // Move to next state
        CaptureStep = ECaptureStep::Capturing;
        FrameCounter = 0;
        break;
    }
    case ECaptureStep::Capturing:
    {
        ++FrameCounter;
        if (FrameCounter >= CaptureData.FramesToKeepInSamePosition)
        {
            CaptureDataNow();
 
            // Prepare for next wait before moving again
            CaptureStep = ECaptureStep::WaitingAfterCapture;
            FrameCounter = 0;
        }
        break;
    }
    case ECaptureStep::WaitingAfterCapture:
    {
        ++FrameCounter;
        if (FrameCounter >= CaptureData.FramesToKeepInSamePosition)
        {
            // Move to next position
            CaptureData.CurrentCaptureIndex++;
            CaptureStep = ECaptureStep::WaitingForPositionUpdate;
            FrameCounter = 0;
        }
        break;
    }
    }
}
 
void ADataCapture::UpdatePositions(const FTransform& Transform)
{
    for (int32 i = 0; i < CaptureData.Sensors.Num(); i++)
    {
        ASensor* SensorPtr = CaptureData.Sensors[i];
        if (SensorPtr)
        {
            SensorPtr->SetActorTransform(Transform);
        }
    }
 
    for (int32 i = 0; i < CaptureData.Lidars.Num(); i++)
    {
        ALidar* LidarPtr = CaptureData.Lidars[i];
        if (LidarPtr)
        {
            // The Lidar sensor generates a full point cloud across multiple frames, 
            // so to ensure accurate data capture after moving the actor, we force a reset of the point cloud capture.
            LidarPtr->ForceClearContainers();
        }
    }
}
 
void ADataCapture::CaptureDataNow()
{
    for (int32 i = 0; i < CaptureData.Cameras.Num(); i++)
    {
        ACamera* CameraPtr = CaptureData.Cameras[i];
        if (CameraPtr)
        {
            CameraPtr->SaveCurrentFrameToDisk();
        }
    }
 
    for (int32 i = 0; i < CaptureData.Lidars.Num(); i++)
    {
        ALidar* LidarPtr = CaptureData.Lidars[i];
        if (LidarPtr)
        {
            LidarPtr->SaveCurrentPointCloudToDisk();
        }
    }
}
 
void ADataCapture::DestroySensors()
{
    for (int32 i = 0; i < CaptureData.Sensors.Num(); i++)
    {
        ASensor* SensorPtr = CaptureData.Sensors[i];
        if (SensorPtr)
        {
            SensorPtr->Destroy();
        }
    }
}
 
void ADataCapture::ComputeAndSetZFromGround(FTransform& Transform)
{
    UWorld* World = GetWorld();
    if (!World)
    {
        return;
    }
 
    FVector Position = Transform.GetLocation();
    FVector Start = Position;
    FVector End = Position - FVector(0, 0, 10000); // Trace downwards
 
    FHitResult HitResult;
    FCollisionQueryParams QueryParams;
 
    bool Hit = World->LineTraceSingleByChannel(HitResult, Start, End, ECC_Visibility, QueryParams);
 
    if (Hit)
    {
        float HeightAboveGround = Position.Z;
        Position.Z = HitResult.ImpactPoint.Z + (HeightAboveGround * 100.0f);
        Transform.SetLocation(Position);
    }
}