raytrixdll-v6/RaytrixStreamer.h

#pragma once
/* ----------------------
* Copyright 2023 Université Libre de Bruxelles(ULB), Universidad Politécnica de Madrid(UPM), CREAL, Deutsches Zentrum für Luft - und Raumfahrt(DLR)
 
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at < http://www.apache.org/licenses/LICENSE-2.0%3E
 
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissionsand
* limitations under the License.
---------------------- */
 
// CREAL - HOVITRON Pipeline header
#ifdef _WIN32
#include <filesystem>
namespace fs = std::filesystem;
#include <Windows.h>
#define HVT_ENTRY_ATTR __declspec(dllexport)
#else
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
typedef void* HANDLE;
#endif
 
#define HVT_FUNCTION_PROTOTYPES
#include "HvtStreamingAPI.h"
 
// C/C++ header
#include <conio.h>
#include <iostream>
#include <thread>
#include <mutex>
#include <atomic>
#include <array>
#include <fstream>
 
// Raytrix Header
#include "Rx.Core\RxException.h"
#include "Rx.LFR/LightFieldRuntime.h"
#include "Rx.LFR/CameraServer.h"
#include "Rx.LFR/CalibrationManager.h"
#include "Rx.LFR/Cuda.h"
#include "Rx.LFR/CudaCompute.h"
#include "Rx.LFR/ImageQueue.h"
#include "Rx.LFR\RayFileReader.h"
#include "Rx.Core/EColorMap.h"
#include "Rx.LFR/RayFileWriter.h"
#include "Rx.LFR/SeqFileReader.h"
#include "Rx.LFR/SeqFileWriter.h"
 
// Cuda header
#include <cuda.h>
#include <cuda_runtime.h>
 
// RaytrixStreamer header
#include "types.h"
#include "KernelCuda.cuh"
#include "nlohmann/json.hpp"
 
#ifndef VK_UUID_SIZE
#define VK_UUID_SIZE 16U
#endif
 
enum class HvtHandleType
{
    CONTEXT,
    SEMAPHORE
};
 
template <typename OurType, typename HvtHandle, HvtHandleType htype>
struct Handle
{
    Handle() : type(htype) {}
    static constexpr HvtHandleType staticType = htype;
 
    static OurType* check(HvtHandle hvtHandle)
    {
        Handle* h = reinterpret_cast<Handle*>(hvtHandle);
        if (h && staticType == h->type)
        {
            return static_cast<OurType*>(h);
        }
        else
        {
            throw HvtResult::HVT_ERROR_INVALID_HANDLE;
        }
    }
 
    static OurType* opt_check(HvtHandle hvtHandle)
    {
        if (!hvtHandle)
        {
            return nullptr;
        }
        return check(hvtHandle);
    }
 
    HvtHandle to_handle()
    {
        return reinterpret_cast<HvtHandle>(this);
    }
 
private:
    HvtHandleType type;
};
 
struct Semaphore : public Handle<Semaphore, HvtSemaphore, HvtHandleType::SEMAPHORE>
{
    cudaExternalSemaphore_t sem;
    bool isValid;
};
 
class RaytrixStreamer : public Handle<RaytrixStreamer, HvtStreamingContext, HvtHandleType::CONTEXT>
{
    public:
        /*-------------- Constructor/Destructor */
        RaytrixStreamer(const uint8_t uuid[VK_UUID_SIZE]);
        ~RaytrixStreamer();
        /*--------------------------------------*/
 
        /*---------- Hovitron API C entry points*/
        void enumerateStreamsParameters(uint32_t* streamsCount, HvtRGBDStreamParameters* parameters) const;
        void importStreamImages(const HvtStreamImagesExportInfo& exportInfos);
        void importSemaphore(const HvtSemaphoreExportInfo& exportInfos);
        void destroySemaphore(Semaphore* sem) const;
        void startStreaming();
        void acquireStreamsFrames(const HvtAcquireStreamFramesInfo& infos);
        void releaseStreamsFrames(Semaphore* waitSem);
        void stopStreaming();
        /*--------------------------------------*/
 
        /*--------- Getter*/
        float getMaxDepth();
        float getMinDepth();
        HvtIntrinsicsPerspective getRxIntrinsicParams(size_t idxCam);
        HvtExtrinsics getRxExtrinsicParams(size_t idxCam);
        Rx::LFR::CImageQueue& getCamBuffer(size_t idxCam);
        /*---------------*/
 
private:
    static constexpr auto numSlots = 3;
    using Clock = std::chrono::steady_clock;
 
    /*---------- Hovitron API structure*/
    // Structure for an image slot. The image is given to the vulkan handle through the cudaSurfaceObject_t.
    struct ImageSlot
    {
        cudaSurfaceObject_t cuda_ptr_surf;
        HVT_OPAQUE_HANDLE* vk_handle;
        int32_t size;
    };
    // A stream is a structure that is sent to the Hovitron Pipeline. It gives all necessery information of the camera and the image.
    struct ReadStream
    {
        size_t colorFrameStride;
        size_t colorFrameSize;
        size_t depthFrameStride;
        size_t depthFrameSize;
 
        HvtProjectionType projectionType;
        HvtIntrinsics intrinsics;
        HvtExtrinsics extrinsics;
 
        HvtImageFormat colorFormat;
        HvtImageFormat depthFormat;
 
        int width_color;
        int height_color;
        int width_depth;
        int height_depth;
        float anear;
        float afar;
 
        std::array<ImageSlot, numSlots> colorSlots;
        std::array<ImageSlot, numSlots> depthSlots;
        bool importedColor = false;
        bool importedDepth = false;
 
        Clock::duration framePeriod;
        int frameCount = 1;
        int streamedFrame = -1;
 
        int frameIndex(Clock::duration time) const;
        bool nextFrameReady(Clock::duration time) const;
    };
    /*---------------------------------*/
 
    /*-------------- RaytrixStreamer DLL structure*/
    // Contain all the information about the way the DLL should work.
    struct infoJSON_t {
        // Number of cams
        size_t numCams = 0;
        //If the DLL should force the use of a specific RGB resolution
        bool forceRGBResolution = false;
        //If the DLL should force the use of a specific RGB depth
        bool forceDepthResolution = false;
        // Width of the RGB image
        unsigned int width = 0;
        // Height of the RGB image
        unsigned int height = 0;
        // Width of the Depth image
        unsigned int widthDepth = 0;
        // Height of the Depth image
        unsigned int heightDepth = 0;
        // The ID of the cameras
        unsigned int* uIDs = nullptr;
        // The filename of the compute parameters
        Rx::CRxString filenameComputeParameters = Rx::CRxString();
        // True, if we use sequence files
        bool loadFromRawFile = false;
        // Path of these seq files
        Rx::CRxString* filenameRawFiles = nullptr;
        // True, if a or some specific calibrations is(are) wanted
        bool specificCal = false;
        // The ID of the calibrations
        Rx::CRxString* GUIDsCalib = nullptr;
        // The exposure of the cameras to set (allow of better and faster depth computation)
        float* exposureCams = nullptr;
        // The path of the extrinsic camera parameters
        std::string extrinsicParamsPath = std::string();
    };
    /*--------------------------------------------*/
 
    /*--------- Hovitron API functions*/
 
    void streamingLoop();
    void uploadFrame(size_t streamId, ReadStream& stream);
    void swapStreamingToPending();
    void swapPendingToReading();
    /*--------------------------------*/
 
    /*------- Init Raytrix DLL functions */
 
    void readInfoFromJSON();
    void initRxCams();
    void findVulkanGPU(const uint8_t uuid[VK_UUID_SIZE]);
    void initStreamParameters();
    void initRxCudaCompute();
    void initRxSDK();
    void initArrayCuda();
    void initRxRayFile();
    void initMutex();
    /*---------------------------------*/
 
    /*--------------- RaytrixDLL functions for RGBD acquire and refine tasks*/
 
    void acquireRGBD();
    void refineRGBD();
    void getRGBDFormats();
    void launchAsyncLoader(bool needToWaitthreadToFinish = false);
    void asyncLoaderImage();
    void asyncLoaderImageOneCam(size_t idxCam);
    void asyncLoaderImageFromSeqFiles();
    void asyncLoaderImageFromOneSeqFile(size_t idxCam);
    // A list of mutexex to assure that we do not have race conditions when the buffer image is moved to the CRxImage object.
    std::mutex* loaderMtxs;
    // A list of condition variables that wait until the condition is verified.
    std::condition_variable* loaderCVs;
    // The list of conditions to block/unblock load of new data.
    bool* canLoadImgs;
    // A mutex to assure that we do not have race conditions between the syncrhonization of cameras and the copy of the data in our CUDA arrays.
    std::mutex syncMtx;
    // The condition variable that waits until the condition is verified.
    std::condition_variable syncCV;
    // The condition to block/unblock the copy of the datas.
    bool imgsLoaded = false;
    /*----------------------------------------------------------------------*/
 
    /*---------- Debug functions*/
 
    template<typename T> void writeInFileWindow(T* dataCorrect, T* dataDebug, size_t width, size_t height, size_t sizeWindow, int numCam = 0);
    void debugFloatDepth();
    /*--------------------------*/
 
    /*--------- Hovitron API variables*/
    int vulkanGPU;
    Semaphore* synthesisSem;
    std::vector<ReadStream> readStreams;
    std::atomic_bool running = false;          
    std::thread streamingThread;               
    uint32_t slotReadingIndex = 0;             
    uint32_t slotPendingIndex = 1;             
    uint32_t slotStreamingIndex = 2;           
    std::atomic_bool newDataInPending = false; 
    std::mutex indicesMutex;                   
    std::mutex queueMutex;
    float numFrames = 0.0f;
    /*--------------------------------*/
 
    /*----------------- RaytrixStreamer variables*/
    // CUDA Streams for stream concurrency programming
    cudaStream_t* cudaStreams;
    // CUDA Event to synchronize a stream
    cudaEvent_t* cudaEvents;
    // All information that the DLL needs
    infoJSON_t infoJSON;
    // A list of pointer to camera calibrations
    Rx::LFR::CCalibration** camCalibrations;
    // The camera server class that is able to find attached cameras.
    Rx::LFR::CCameraServer xCamServer;
    // This is the CUDA compute instances
    Rx::LFR::CCudaCompute* xCudaComputes;
    // Two lists of USHORT arrays
    unsigned short** imgRGB, ** imgDepth;
    // Two lists of UCHAR/UBYTE arrays (only RGB image should be in UChar)
    uchar** finalRGB, ** imgUByteRGB;
    // Three lists of float arrays (only depth image should be in Float) 
    float** finalDepth, ** imgFloatDepth, ** prevFinalDepth;;
    // A list of camera buffers
    Rx::LFR::CImageQueue* camBuffers;
    // Capture mode of the cameras
    const Rx::ECamTriggerMode camCaptureMode = Rx::ECamTriggerMode::Camera_FreeRun;
    // Set up the image buffer properties
    const unsigned uBufferSize = 1;
    // Set up the image buffer properties
    const bool bOverwrite = true;
    // List of CRxImage that are used as a tmp image loader (for camera acquired method)
    Rx::CRxImage* capturedImages;
    // List of the format of the RGB and depth images
    Rx::CRxImageFormat* imgFormatRGB, * imgFormatDepth;
    // Cuda image variable to extract the cudaRGB and cudaDepth from the RxSDK
    Rx::LFR::CudaImage* cuImgs, * cuDepths;
    // A boolean to know if the formats have been already loaded
    bool isFormatLoaded = false;
    // A list of CRayImage that are used as a tmp image loader (for Sequence file acquired method)
    Rx::LFR::CRayImage* rayImages;
    // A list of sequence files
    Rx::LFR::CSeqFileReader* seqReaders;
    // The pitch of the CUDA RGB and depth array, pitch = number of bytes in row <=> IdxFromPitch = pitch / size(T) 
    size_t* pitchInCudaRGB, * pitchInCudaDepth;
    // List of reference plane position, allows to where where the reference plane has been put from one camera
    float* referencePlaneToCameraPlane;
    // Size of raytrix by default
    size_t widthDefault = 3840, heightDefault = 2160;
    // Channel of depth in Depth3D array
    const size_t channelDepth = 2;
    // Max time to capture imgs between cams (in ??)
    const double MAX_TIME_BETWEEN_CAMS = 1000.0;
    // Max time to wait for buffer to be filled (in ms)
    const int MAX_TIME_WAIT_BUFFER = 1000;
    // Name of the environment variable allows loading the parameters
    const char* ENV_NAME = "HVT_RXDLL_PARAMS_PATH";
    // ID of the images that we want to compute
    const Rx::LFR::EImage idRGB = Rx::LFR::EImage::TotalFocus_View_Virtual, idDepth = Rx::LFR::EImage::Depth3D_ViewVirtualUndistorted_To_Reference;
    // ID of he space to compute the total focus
    const Rx::ELFSpace idSpaceRGB = Rx::ELFSpace::View_VirtualUndistorted;
    // ID of the space to compute the depth 3D
    const Rx::ELFSpace idSpaceDepth3D[2] = { idSpaceRGB, Rx::ELFSpace::Reference };
    /*-------------------------------------------*/
 
    /*------- Print fps(variables + functions)*/
    // A string that will be used to display message as the FPS
    char* printMsg = NULL;
    // An uint to know where we need to add a new message to the string
    unsigned int whereToAddMsg;
    // A fixed size of the string
    const int SIZE_MSG = 1000;
    // Time when the first loop began
    std::chrono::steady_clock::time_point beginAvgStreaming;
    // Time when the stream has done one loop
    std::chrono::steady_clock::time_point endStreaming;
    void addMsgToPrint(const char* title, long long int averageTime, float frameNumber);
    void resetPrintMsg();
    void printFPS();
    /*----------------------------------------*/
};