DVSCamera.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 "DVSCamera.h"
 
#include "Agrarsense/Game/AgrarsenseStatics.h"
#include "Agrarsense/Logging/SimulatorLog.h"
 
#include "Async/TaskGraphInterfaces.h"
#include "Engine/Texture2D.h"
#include "Async/ParallelFor.h"
 
#include <random>
#include <algorithm>
#include <cstring>
#include <cerrno>
#include <cmath>
#include <cfenv>
 
std::random_device rd;
std::minstd_rand Engine;
 
static constexpr float R_WEIGHT = 0.2989f;
static constexpr float G_WEIGHT = 0.5870f;
static constexpr float B_WEIGHT = 0.1140f;
static constexpr float INV_MAX_VALUE = 1.0f / 255.0f;
 
ADVSCamera::ADVSCamera(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    // Cannot tick parallel due UTexture2D locks
    UseParallelLateTick = false;
 
    PrimaryActorTick.bCanEverTick = true;
    PrimaryActorTick.TickGroup = TG_PostUpdateWork;
}
 
void ADVSCamera::DVSInit(FDVSCameraParameters Parameters, bool SimulateSensor)
{
    DVSCameraParameters = Parameters;
    Init(DVSCameraParameters.CameraParameters, SimulateSensor);
}
 
void ADVSCamera::Init(FCameraBaseParameters parameters, bool SimulateSensor)
{
    // Defined in Camera.h
    CameraName = "DVSCamera ";
    FilePrefix = "Data/DVSCamera_";
 
    Super::Init(DVSCameraParameters.CameraParameters, SimulateSensor);
 
    const int32 Width = GetCameraWidth();
    const int32 Height = GetCameraHeight();
    const int32 Size = Width * Height;
 
    auto DVSEventFill = DVSEvent();
    EventArray.resize(Size, DVSEventFill);
 
    DVSOutput.Init(FColor::Black, Size);
 
    this->OnCameraWindowResized.AddUniqueDynamic(this, &ADVSCamera::OnMainCameraWindowResized);
 
    if (!DVSCameraParameters.VisualizeDVSCamera)
    {
        return;
    }
 
    // Since the Camera.cpp sensor works by USceneCaptureComponent2D rendering into custom Window texture,
    // where its pixels are read through FrameGrabber API,
    // We are unable (or at least I couldn't figure a way) to manipulate the pixels 
    // AND visualize it in the same Window texture without flickering.
    // So here we can optionally create another window where we can visualize the DVS camera output.
    // This Window can be closed safely and it does not destroy the sensor itself, and it will not stop sending data to ROS.
 
    // Create another Window for DVS visualization.
    FString windowName = GetSensorIdentifier() + " Output";
    DVSUnrealWindow = MakeShareable(new FUnrealWindow(Width, Height, windowName));
    if (DVSUnrealWindow.IsValid())
    {
        // Only draw when requested.
        DVSUnrealWindow->SetAutomaticDraw(false);
 
        OnDVSWindowClosed.BindUObject(this, &ADVSCamera::DVSWindowClosedCallback);
        DVSUnrealWindow->GetSWindow()->SetOnWindowClosed(OnDVSWindowClosed);
        DVSUnrealWindow->ResizeWindow(Width, Height);
    }
 
#ifdef EngineAllowRenderingMinimized
    // Minimize the DVS camera window (which just shows the RGB camera image without DVS conversion),
    // so we are only left with DVS window
    if (UnrealWindow.IsValid())
    {
        UnrealWindow->GetSWindow()->Minimize();
    }
#endif
 
    // Setup PointCloudMessage
    PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
    PointCloudMessage->header.frame_id = "world";
    PointCloudMessage->height = 1;
    PointCloudMessage->is_dense = true;
 
    PointCloudMessage->fields.Reserve(4);
 
    PointCloudMessage->fields.Add({ "x", 0, ROSMessages::sensor_msgs::PointCloud2::PointField::UINT16, 1 });
    PointCloudMessage->fields.Add({ "y", 2, ROSMessages::sensor_msgs::PointCloud2::PointField::UINT16, 1 });
    PointCloudMessage->fields.Add({ "t", 4, ROSMessages::sensor_msgs::PointCloud2::PointField::UINT32, 1 });
    PointCloudMessage->fields.Add({ "pol", 8, ROSMessages::sensor_msgs::PointCloud2::PointField::UINT8, 1 });
 
    PointCloudMessage->point_step = 9;
}
 
void ADVSCamera::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    if (DVSUnrealWindow.IsValid())
    {
        OnDVSWindowClosed.Unbind();
        DVSUnrealWindow->DestroyWindow();
        DVSUnrealWindow.Reset();
    }
 
    this->OnCameraWindowResized.RemoveDynamic(this, &ADVSCamera::OnMainCameraWindowResized);
 
    LastEventTimestamp.Empty();
    PreviousImage.Empty();
    LastImage.Empty();
    RefValues.Empty();
    DVSOutput.Empty();
 
    EventArray.clear();
 
    PointCloudMessage.Reset();
 
    if (DvsTopic)
    {
        DvsTopic->Unadvertise();
        DvsTopic->Unsubscribe();
        DvsTopic->MarkAsDisconnected();
        DvsTopic->ConditionalBeginDestroy();
        DvsTopic = nullptr;
    }
}
 
void ADVSCamera::OnMainCameraWindowResized(ACamera* Camera, FCameraBaseParameters Params)
{
    TempDVSCameraParameters = DVSCameraParameters;
    SetParentParameters = false;
    ParametersChanged = true;
}
 
void ADVSCamera::ChangeDVSCameraParameters(FDVSCameraParameters NewParameters)
{
    TempDVSCameraParameters = NewParameters;
    SetParentParameters = true;
 
    if (!CanSimulateSensor())
    {
        ChangeParametersInternal();
    }
    else
    {
        ParametersChanged = true;
    }
}
 
void ADVSCamera::DVSWindowClosedCallback(const TSharedRef<SWindow>& Window)
{
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("DVSCamera.cpp: DVS Camera output window closed."));
#endif
}
 
float ADVSCamera::FColorToGrayScaleFloat(const FColor& Color)
{
    return R_WEIGHT * Color.R + G_WEIGHT * Color.G + B_WEIGHT * Color.B;
}
 
void ADVSCamera::ImageToGray(const TArray<FColor>& Buffer)
{
    LastImage.SetNumUninitialized(Buffer.Num());
 
    // Convert image to gray image values
    ParallelFor(Buffer.Num(), [&](int32 i)
        {
            LastImage[i] = FColorToGrayScaleFloat(Buffer[i]);
        }, !DVSCameraParameters.ParalellImageConversion);
}
 
void ADVSCamera::ImageToLogGray(const TArray<FColor>& Buffer)
{
    const int32 Size = Buffer.Num();
    const float LogEps = DVSCameraParameters.LogEps;
 
    LastImage.SetNumUninitialized(Size);
 
    // Convert FColors to logarithmic grayscale
    ParallelFor(Size, [&](int32 Index)
        {
            float GrayScale = FColorToGrayScaleFloat(Buffer[Index]) * INV_MAX_VALUE;
            LastImage[Index] = std::log(LogEps + GrayScale);
        }, !DVSCameraParameters.ParalellImageConversion); // Force single thread? Default false.
}
 
std::vector<DVSEvent> ADVSCamera::SimulateDVS(const TArray<FColor>& Buffer)
{
    std::vector<DVSEvent> Events;
 
    const int32 Width = GetCameraWidth();
    const int32 Height = GetCameraHeight();
    const int32 Size = Width * Height;
 
    if (DVSOutput.Num() != Size)
    {
        DVSOutput.Empty();
        DVSOutput.SetNumUninitialized(Size);
        DVSOutput.Init(FColor::Black, Size);
 
        PreviousImage.Empty();
        PreviousImage.SetNumZeroed(0);
    }
 
    if (Buffer.Num() != Size)
    {
        return Events;
    }
 
    if (DVSCameraParameters.UseLog)
    {
        ImageToLogGray(Buffer);
    }
    else
    {
        ImageToGray(Buffer);
    }
 
    UWorld* World = GetWorld();
 
    if (PreviousImage.Num() == 0)
    {
        // Set the first rendered image
        RefValues = LastImage;
        PreviousImage = LastImage;
 
        // Resizes array to given number of elements. New elements will be zeroed.
        LastEventTimestamp.SetNumZeroed(LastImage.Num());
 
        // Reset current time
        CurrentTime = SecToNanoSec(World->GetTimeSeconds());
 
        return Events;
    }
 
    EventArray.clear();
 
    auto DVSEventFill = DVSEvent();
    DVSEventFill.x = 100;
    DVSEventFill.y = 100;
    DVSEventFill.t = 100;
    EventArray.resize(Size, DVSEventFill);
 
    FColor desiredColor = FColor::Black;
    FMemory::Memset(DVSOutput.GetData(), desiredColor.ToPackedARGB(), DVSOutput.Num() * sizeof(FColor));
 
    static constexpr float tolerance = 1e-6;
 
    const std::uint64_t delta_t_ns = SecToNanoSec(World->GetTimeSeconds()) - CurrentTime;
 
    const float cp = DVSCameraParameters.PositiveThreshold;
    const float cm = DVSCameraParameters.NegativeThreshold;
    const float sigma_cp = DVSCameraParameters.SigmaPositiveThreshold;
    const float sigma_cm = DVSCameraParameters.SigmaNegativeThreshold;
    const float refractory_period_ns = DVSCameraParameters.RefractoryPeriodNs;
 
    // TODO: This could improved.
    for (size_t y = 0; y < Height; y++)
    {
        for (int32 x = 0; x < Width; ++x)
        {
            const uint32 i = (Width * y) + x;
            const float itdt = LastImage[i];
            const float it = PreviousImage[i];
            const float prev_cross = RefValues[i];
 
            if (std::fabs(it - itdt) > tolerance)
            {
                const float pol = (itdt >= it) ? +1.0 : -1.0;
                float C = (pol > 0) ? cp : cm;
                const float sigma_C = (pol > 0) ? sigma_cp : sigma_cm;
 
                if (sigma_C > 0)
                {
                    C += std::normal_distribution<float>(0, sigma_C)(Engine);
                    constexpr float minimum_contrast_threshold = 0.01;
                    C = std::max(minimum_contrast_threshold, C);
                }
                float curr_cross = prev_cross;
                bool all_crossings = false;
                do
                {
                    curr_cross += pol * C;
 
                    if ((pol > 0 && curr_cross > it && curr_cross <= itdt)
                        || (pol < 0 && curr_cross < it && curr_cross >= itdt))
                    {
                        const std::uint64_t edt = (curr_cross - it) * delta_t_ns / (itdt - it);
                        const std::int64_t t = CurrentTime + edt;
 
                        // check that pixel (x,y) is not currently in a "refractory" state
                        // i.e. |t-that last_timestamp(x,y)| >= refractory_period
                        const std::int64_t last_stamp_at_xy = SecToNanoSec(LastEventTimestamp[i]);
                        if (t >= last_stamp_at_xy)
                        {
                            const std::uint64_t dt = t - last_stamp_at_xy;
                            if (LastEventTimestamp[i] == 0 || dt >= refractory_period_ns)
                            {
                                bool val = pol > 0;
 
                                EventArray[i] = DVSEvent(x, y, t, val);
                                DVSOutput[i] = val ? FColor::Red : FColor::Black;
 
                                LastEventTimestamp[i] = NanoSecToSecTrunc(t);
                            }
                            RefValues[i] = curr_cross;
                        }
                    }
                    else
                    {
                        all_crossings = true;
                    }
                } while (!all_crossings);
            }
        }
    }
 
    // Update current time
    CurrentTime = SecToNanoSec(World->GetTimeSeconds());
 
    PreviousImage = LastImage;
 
    // Remove default values from EventArray std::vector
    EventArray.erase(std::remove(EventArray.begin(), EventArray.end(), DVSEventFill), EventArray.end());
 
    if (DVSCameraParameters.SortByIncreasingTimestamp)
    {
        // Sort the events by increasing timestamps
        std::sort(EventArray.begin(), EventArray.end(), [](const DVSEvent& it1, const DVSEvent& it2)
            {
                return it1.t < it2.t;
            });
    }
 
    return EventArray;
}
 
void ADVSCamera::CreateROSTopic()
{
    if (!ROSInstance || !IsROSConnected())
    {
        return;
    }
 
    FString ID = GetSensorIdentifier();
 
    if (!DvsTopic)
    {
        FString RawDataTopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s_raw"), *ID);
 
        DvsTopic = NewObject<UTopic>(UTopic::StaticClass());
        DvsTopic->Init(ROSInstance->ROSIntegrationCore, RawDataTopicName, TEXT("sensor_msgs/PointCloud2"));
        DvsTopic->Advertise();
 
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("DVSCamera.cpp: Created DVS Camera raw data ROS topic. Topic name: %s"), *RawDataTopicName);
#endif
    }
 
    if (!ROSTopic)
    {
        FString ImageTopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s"), *ID);
 
        ROSTopic = NewObject<UTopic>(UTopic::StaticClass());
        ROSTopic->Init(ROSInstance->ROSIntegrationCore, ImageTopicName, TEXT("sensor_msgs/Image"));
        ROSTopic->Advertise();
 
#if WITH_EDITOR
        UE_LOG(LogTemp, Warning, TEXT("DVSCamera.cpp: Created DVS Camera Image ROS topic. Topic name: %s"), *ImageTopicName);
#endif
    }
}
 
void ADVSCamera::SendRawDVSImageDataToROS()
{
    if (EventArray.size() == 0
        || !SendDataToROS
        || !DvsTopic
        || !IsROSConnected()
        || !PointCloudMessage.IsValid())
    {
        return;
    }
 
    PointCloudMessage->width = EventArray.size();
    PointCloudMessage->row_step = PointCloudMessage->width * PointCloudMessage->point_step;
 
    const DVSEvent* PointsDataPtr = EventArray.data();
    PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<DVSEvent*>(PointsDataPtr));
 
    // Send raw DVS data (PointCloud2) to ROS Topic
    DvsTopic->Publish(PointCloudMessage);
}
 
void ADVSCamera::AddProcessingToFrameBuffer(TArray<FColor>& buffer)
{
    SimulateDVS(buffer);
 
    FGraphEventRef SendDataToROSTask = FFunctionGraphTask::CreateAndDispatchWhenReady([this]()
        {
            SendRawDVSImageDataToROS();
        }, TStatId(), nullptr, ENamedThreads::AnyBackgroundHiPriTask);
 
    // Bind DVSOutput to buffer
    buffer = DVSOutput;
 
    // Update secondary Window with the new DVS image
    UpdateDVSWindowOutput(buffer);
 
    if (SendDataToROSTask.IsValid())
    {
        FTaskGraphInterface::Get().WaitUntilTaskCompletes(SendDataToROSTask);
    }
 
    if (ParametersChanged)
    {
        ChangeParametersInternal();
    }
}
 
void ADVSCamera::ChangeParametersInternal()
{
    ParametersChanged = false;
    DVSCameraParameters = TempDVSCameraParameters;
 
    if (SetParentParameters)
    {
        SetParentParameters = false;
 
        // Change FCameraBaseParameters
        ChangeCameraParameters(DVSCameraParameters.CameraParameters);
    }
    else
    {
        if (DVSUnrealWindow.IsValid())
        {
            const int32 Width = GetCameraWidth();
            const int32 Height = GetCameraHeight();
            DVSUnrealWindow->ResizeWindow(Width, Height);
        }
    }
}
 
void ADVSCamera::UpdateDVSWindowOutput(const TArray<FColor>& Buffer)
{
    if (!DVSUnrealWindow.IsValid() || !CaptureFrameTexture)
    {
        return;
    }
 
    // CaptureFrameTexture defined in Camera.h and it should 
    // resize automatically in Camera.cpp if parent Window is resized.
 
    // Lock texture
    uint8* TextureData = static_cast<uint8*>(CaptureFrameTexture->GetPlatformData()->Mips[0].BulkData.Lock(LOCK_READ_WRITE));
 
    // Copy buffer to texture
    FMemory::Memcpy(TextureData, Buffer.GetData(), Buffer.Num() * sizeof(FColor));
 
    // Unlock texture
    CaptureFrameTexture->GetPlatformData()->Mips[0].BulkData.Unlock();
 
    // Update texture resource
    CaptureFrameTexture->UpdateResource();
    FTextureResource* resource = CaptureFrameTexture->GetResource();
 
    // Update new texture to window and draw
    DVSUnrealWindow->UpdateTexture(resource);
    DVSUnrealWindow->DrawManually();
}