raytrixdll-v5/RaytrixStreamer.cpp

#include "RaytrixStreamer.h"
 
/* ---------------------------
 
    Static functions for Raytrix callback functions 
 
*/
 
// Copyright (c) 2019 Raytrix GmbH. All rights reserved.
// 
void OnImageCaptured(const Rx::CRxImage& xImage, unsigned uCamIdx, Rx::LFR::CImageQueue& camBuffer)
{
    try
    {
        // Make a copy of the provided image. We don't know if this image is reused by the camera SDK or by another handler
        Rx::CRxImage xCapturedImage;
        xCapturedImage.Create(xImage);
 
        /************************************************************************/
        /* Write into buffer                                                    */
        /************************************************************************/
        if (!camBuffer.MoveIn(std::move(xCapturedImage)))
        {
            // Buffer is full and overwrite is disabled
            // This is a lost frame
            return;
        }
    }
    catch (Rx::CRxException& ex)
    {
        printf("Exception occured:\n%s\n\n", ex.ToString(true).ToCString());
        printf("Press any key to end program...\n");
        //(void)_getch();
    }
}
static void ImageCaptured(const Rx::CRxImage& xImage, unsigned uCamIdx, void* pvContext)
{
    RaytrixStreamer* context = (RaytrixStreamer*)pvContext;
    OnImageCaptured(xImage, uCamIdx, context->getCamBuffer(uCamIdx));
}
/* 
 
    Static functions for Raytrix callback functions
 
 --------------------------- */
 
 
 /* ----------------------
 * 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.
 ---------------------- */
 
/* ----------------------------
    The Raytrix DLL has been written by Armand Losfeld (armand.losfeld@ulb.be or armand-losfeld@hotmail.com) with the help of Martin Lingenauber.
    This work has been highly inspired from the work made for the Kinect DLL (AcqKiRT) written by Jaime Sancho.
 ---------------------------- */
 
/* ------------------------------------
 
    RaytrixStreamer class functions
 
*/
 
/*------- Init Raytrix DLL functions */
 
void RaytrixStreamer::findVulkanGPU(const uint8_t uuid[VK_UUID_SIZE]) {
    // Find vulkan GPU (synthesis GPU)
    int cudaDeviceCount;
    cudaGetDeviceCount(&cudaDeviceCount);
 
    vulkanGPU = NULL;
    for (int cudaDevice = 0; cudaDevice < cudaDeviceCount; cudaDevice++)
    {
        cudaDeviceProp deviceProp;
        cudaGetDeviceProperties(&deviceProp, cudaDevice);
 
        std::cout << "GPU n: " << cudaDevice << std::endl;
        const unsigned char* p = reinterpret_cast<uchar*>(&deviceProp.uuid);
        for (size_t i = 0; i < 16; i++) {
            printf("%02hhx", p[i]);
        }
        putchar('\n');
 
        if (!memcmp(&deviceProp.uuid, uuid, VK_UUID_SIZE))
        {
            vulkanGPU = cudaDevice;
        }
    }
    std::cout << std::endl;
    std::cout << "Vulkan GPU: " << std::endl;
    for (size_t i = 0; i < 16; ++i) {
        printf("%02hhx", uuid[i]);
    }
    putchar('\n');
 
    std::cout << std::endl;
 
 
    gpuErrchk(cudaSetDevice(vulkanGPU));
    std::cout << "Available GPUs: " << cudaDeviceCount << " Vulkan GPU:" << vulkanGPU << std::endl;
}
 
void RaytrixStreamer::initRxCams() {
    // Init Rx Cameras
    std::cout << "Initializing Rx cams" << std::endl;
 
    // Start to find cameras. This is an synchronous call and we wait here until the find process has been finished
    xCamServer.FindCameras();
 
    // Quit the application if there is no camera
    if (xCamServer.GetCameraCount() == 0)
    {
        printf("No camera found\n");
        throw HVT_ERROR_NOT_FOUND;
    }
 
    printf("Number of cameras available: %d\n", xCamServer.GetCameraCount());
    
    for (int i = 0; i < infoJSON.numCams; ++i) {
        // Get the camera from the camera server
        auto& xCam = xCamServer.GetCamera(infoJSON.uIDs[i]);
 
        // Open the camera
        printf("Opening camera %i...", infoJSON.uIDs[i]);
        xCam.Open();
        printf("done.\n");
 
        // Add a image captured callback. This method gets called for every captured camera image and more details are given there
        xCam.AddImageCapturedCallback(ImageCaptured, (void*) this);
 
        printf("______________________________________________\n");
        printf("Camera %d:\n", infoJSON.uIDs[i]);
 
        // Write the camera type to the console, you can find your camera type on the label of your camera.
        // E.g. R26-C-A-U3-A028-RS-A
        // The type is given by the 3. entry: R26-C- ->A<- -U3-A028-RS-A
        // This does not represent the camera connection to your current device, but for this camera example it is USB 3.0 indicated by the U3
        printf(">> Type: %s\n", xCam.GetDriverName().ToCString());
 
        // This is the camera serial printed on your camera label
        printf(">> ID  : %s\n", xCam.GetInternalSerial().ToCString());
 
        printf(">> Buffersize: %u\n", uBufferSize);
 
        printf(">> Overwrite  : %s\n", bOverwrite ? "Yes" : "No");
 
        printf("\n\n");
 
        // Create buffer within the given size and with the given overwrite flag
        camBuffers[i].Initialize(uBufferSize, bOverwrite);
 
        if(xCam.IsPropertyAvailable(Rx::Interop::Runtime30::Camera::EProperty::Exposure)) xCam.SetProperty(Rx::Interop::Runtime30::Camera::EProperty::Exposure, infoJSON.exposureCams[i]);
 
        Rx::CRxArrayString GUIDs;
        Rx::LFR::CCalibrationManager::GetCalibrationGUIDs(GUIDs);
        int idxInGUIDs = -1;
        for (size_t j = 0; infoJSON.specificCal && j < GUIDs.Length(); ++j) {
            if (GUIDs.GetDataPtr()[j] == infoJSON.GUIDsCalib[i]) {
                std::cout << "Find metric calibration wanted: " << infoJSON.GUIDsCalib[i].ToCString() << std::endl;
                idxInGUIDs = j;
            }
        }
 
        if (idxInGUIDs >= 0) {
            Rx::LFR::CCalibrationManager::LoadCalibration(*(camCalibrations[i]), GUIDs.GetDataPtr()[idxInGUIDs], true);
        }
        else {
            // Load the default calibration of the camera (and load the gray image too)
            if (!Rx::LFR::CCalibrationManager::HasDatabase(xCam))
            {
                std::cerr << "The first camera in your system does not have a database." << std::endl;
                std::cerr << "Either use the installer for your camera settings to install the calibration on your system or create a calibration manualy via RxLive for example." << std::endl;
                throw HVT_ERROR_NOT_FOUND;
            }
            Rx::LFR::CCalibrationManager::LoadDefaultCalibration(*(camCalibrations[i]), xCam, true);
        }
    }
}
 
void RaytrixStreamer::initStreamParameters() {
 
    synthesisSem = new Semaphore();
    if (synthesisSem == NULL) {
        std::cerr << "Impossible to allocate data for semaphore ..." << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
 
    // Init Creal params
    auto frameRate = 30;
    using namespace std::chrono_literals;
    auto framePeriod = std::chrono::nanoseconds(1s) / frameRate;
 
    //Stream for each cam
    for (size_t i = 0; i < infoJSON.numCams; ++i)
    {
        HvtIntrinsics intrinsics;
        HvtProjectionType ptype;
 
        // Always perspective projection
        ptype = HvtProjectionType::HVT_PROJECTION_PERSPECTIVE;
        intrinsics.perspective = getRxIntrinsicParams(i);
 
        HvtExtrinsics extrinsics = getRxExtrinsicParams(i);
 
        ReadStream RS;
        RS.projectionType = ptype;
        RS.width_color = infoJSON.width;
        RS.height_color = infoJSON.height;
        RS.width_depth = infoJSON.widthDepth;
        RS.height_depth = infoJSON.heightDepth;
        RS.afar = getMaxDepth();
        RS.anear = getMinDepth();
        RS.colorFrameSize = infoJSON.width * infoJSON.height * 4; // RGBA
        RS.colorFrameStride = infoJSON.width * 4;
        RS.depthFrameSize = infoJSON.widthDepth * infoJSON.heightDepth * sizeof(float); // F32
        RS.depthFrameStride = infoJSON.widthDepth * sizeof(float);
        RS.colorFormat = HvtImageFormat::HVT_FORMAT_R8G8B8A8_UNORM; // HVT_FORMAT_G8_B8_R8_3PLANE_420_UNORM
        RS.depthFormat = HvtImageFormat::HVT_FORMAT_R32_SFLOAT;
        RS.intrinsics = intrinsics;
        RS.extrinsics = extrinsics;
        RS.framePeriod = framePeriod;
        RS.frameCount = (int) numFrames;
        readStreams.push_back(RS);
    }
}
 
void RaytrixStreamer::initRxCudaCompute() {
    std::cout << "Initialize RxCUDACompute instances" << std::endl;
    xCudaComputes = new Rx::LFR::CCudaCompute[infoJSON.numCams];
    // Enumerate all CUDA devices at the beginning
    Rx::LFR::CCuda::EnumerateCudaDevices();
    Rx::CRxMetaData metaData;
    double imgDivisor;
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        try {
            xCudaComputes[i].SetCudaDevice(Rx::LFR::CCuda::GetDevice(vulkanGPU));
            // Apply the calibration to the compute instance and set the DataType for the processed images
            xCudaComputes[i].ApplyCalibration(*(camCalibrations[i]), true);
 
            if (infoJSON.loadFromRawFile) {
                seqReaders[i].GetMetaData((Rx::CRxMetaData&)metaData);
                xCudaComputes[i].GetComputeParams().ImportFromMetaData(metaData);
                
                rayImages[i].GetSize((int&)widthDefault, (int&)heightDefault);
            }
            else {
                xCudaComputes[i].GetComputeParams().ImportParameterFromFile(infoJSON.filenameComputeParameters);
 
                xCamServer.GetCamera(i).GetProperty(Rx::Interop::Runtime30::Camera::EProperty::Width, (int&)widthDefault);
                xCamServer.GetCamera(i).GetProperty(Rx::Interop::Runtime30::Camera::EProperty::Height, (int&)heightDefault);
            }
 
            xCudaComputes[i].GetComputeParams().SetValue(Rx::LFR::Params::ECudaCompute::PreProc_DataType, (unsigned)Rx::Interop::Runtime28::EDataType::UByte);
            
            if (infoJSON.forceRGBResolution) {
                if (widthDefault / infoJSON.width != heightDefault / infoJSON.height) {
                    std::cout << "The focus image divisor should be the same for the rows and the the columns..." << std::endl;
                }
                imgDivisor = (double)widthDefault / (double)infoJSON.width;
                xCudaComputes[i].GetComputeParams().SetValue(Rx::LFR::Params::ECudaCompute::Focus_ImageDivisor, imgDivisor);
            }
            else {
                xCudaComputes[i].GetComputeParams().GetValue(Rx::LFR::Params::ECudaCompute::Focus_ImageDivisor,(double&) imgDivisor);
                infoJSON.width = widthDefault/imgDivisor;
                infoJSON.height = heightDefault/imgDivisor;
            }
 
            if (infoJSON.forceDepthResolution) {
                if (widthDefault / infoJSON.widthDepth != heightDefault / infoJSON.heightDepth) {
                    std::cout << "The depth image divisor should be the same for the rows and the the columns..." << std::endl;
                }
                imgDivisor = (double)widthDefault / (double)infoJSON.widthDepth;
                xCudaComputes[i].GetComputeParams().SetValue(Rx::LFR::Params::ECudaCompute::Depth_ImageDivisor, imgDivisor);
            }
            else {
                xCudaComputes[i].GetComputeParams().GetValue(Rx::LFR::Params::ECudaCompute::Depth_ImageDivisor, (double&)imgDivisor);
                infoJSON.widthDepth = widthDefault / imgDivisor;
                infoJSON.heightDepth = heightDefault / imgDivisor;
            }
        }
        catch (Rx::CRxException e) {
            std::cerr << "Failed to initialized cuda compute... Check Raytrix error code: " << e.ToString().ToCString() << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
    }
 
}
 
void RaytrixStreamer::initRxSDK() {
    // Authenticate the Light Field Runtime
    printf("Authenticate LFR...\n");
    try {
        Rx::LFR::CLightFieldRuntime::Authenticate();
    }
    catch (Rx::CRxException e) {
        std::cerr << "Impossible to authenticate. Check RaytrixSDK error code: " << e.ToString().ToCString() << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
    if (!Rx::LFR::CLightFieldRuntime::IsFeatureSupported(Rx::Dongle::ERuntimeFeature::SDK)) {
        std::cerr << "Your license does not allow you to use the SDK properly. Upgrade it or buy another license..." << std::endl;
        throw HVT_ERROR_INVALID_HANDLE;
    }
 
    camCalibrations = (Rx::LFR::CCalibration**)malloc(sizeof(Rx::LFR::CCalibration*) * infoJSON.numCams);
    if (camCalibrations == NULL) {
        std::cerr << "Impossible to allocate memory for default calibrations" << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
 
    if (infoJSON.loadFromRawFile) {
        rayImages = new  Rx::LFR::CRayImage[infoJSON.numCams];
        seqReaders = new Rx::LFR::CSeqFileReader[infoJSON.numCams];
    }
    else {
        camBuffers = new Rx::LFR::CImageQueue[infoJSON.numCams];
        capturedImages = new Rx::CRxImage[infoJSON.numCams];
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            camCalibrations[i] = new Rx::LFR::CCalibration();
        }
    }
    imgFormatRGB = new Rx::CRxImageFormat[infoJSON.numCams];
    imgFormatDepth = new Rx::CRxImageFormat[infoJSON.numCams];
 
    pitchInCudaRGB = new size_t[infoJSON.numCams];
    pitchInCudaDepth = new size_t[infoJSON.numCams];
 
    referencePlaneToCameraPlane = new float[infoJSON.numCams];
}
 
void RaytrixStreamer::initArrayCuda() {
    std::cout << "Init CUDA array" << std::endl;
    imgRGB = (unsigned short**)malloc(sizeof(unsigned short*) * infoJSON.numCams);
    imgDepth = (unsigned short**)malloc(sizeof(unsigned short*) * infoJSON.numCams);
    finalRGB = (unsigned char**)malloc(sizeof(unsigned char*) * infoJSON.numCams);
    finalDepth = (float**)malloc(sizeof(float*) * infoJSON.numCams);
    imgFloatDepth = (float**)malloc(sizeof(float*) * infoJSON.numCams);
    prevFinalDepth = (float**)malloc(sizeof(float*) * infoJSON.numCams);
    imgUByteRGB = (uchar**)malloc(sizeof(uchar*) * infoJSON.numCams);
 
    if (imgRGB == NULL || imgDepth == NULL || finalRGB == NULL || finalDepth == NULL) {
        std::cerr << "Impossible to allocate data for img vectors..." << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
 
    gpuErrchk(cudaSetDevice(vulkanGPU));
    cudaStreams = (cudaStream_t*)malloc(sizeof(cudaStream_t) * infoJSON.numCams);
    cudaEvents = (cudaEvent_t*)malloc(sizeof(cudaEvent_t) * infoJSON.numCams);
 
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        gpuErrchk(cudaMalloc(finalRGB + i, sizeof(uchar) * infoJSON.width * infoJSON.height * 4));
        gpuErrchk(cudaMalloc(finalDepth + i, sizeof(float) * infoJSON.widthDepth * infoJSON.heightDepth));
        gpuErrchk(cudaMalloc(prevFinalDepth + i, sizeof(float) * infoJSON.widthDepth * infoJSON.heightDepth));
 
        gpuErrchk(cudaStreamCreate(&(cudaStreams[i])));
        gpuErrchk(cudaEventCreateWithFlags(&(cudaEvents[i]), cudaEventDisableTiming));
    }
}
 
void RaytrixStreamer::readInfoFromJSON() {
    char* jsonPath;
    size_t len;
    errno_t err = _dupenv_s(&jsonPath, &len, ENV_NAME);
    if (err == -1) {
        std::cerr << "No " << ENV_NAME << "env variable set, cannot load settings" << std::endl;
        throw HvtResult::HVT_ERROR_NOT_FOUND;
    }
 
    // read json file
    std::cout << "Start reading JSON file..." << std::endl;
    std::ifstream jsonRead{ jsonPath, std::ifstream::binary };
    if (!jsonRead.good()) {
        std::cout << "Impossible to read JSON file, check path... Path given: " << jsonPath << std::endl;
        throw HVT_ERROR_NOT_FOUND;
    }
    nlohmann::json jsonContent;
    jsonRead >> jsonContent;
    // Num of cams
    infoJSON.numCams = jsonContent["numCams"];
    // Specific RGB res
    infoJSON.forceRGBResolution = jsonContent["forceUseOfSpecificRBGResolution"];
    if (infoJSON.forceRGBResolution) {
        // Size total focus
        infoJSON.width = jsonContent["width"];
        infoJSON.height = jsonContent["height"];
    }
    // Specific depth res
    infoJSON.forceDepthResolution = jsonContent["forceUseOfSpecificDepthResolution"];
    if (infoJSON.forceRGBResolution) {
        // Size depth
        infoJSON.widthDepth = jsonContent["width_depth"];
        infoJSON.heightDepth = jsonContent["height_depth"];
    }
    // ID cams
    infoJSON.uIDs = new unsigned int[infoJSON.numCams];
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        infoJSON.uIDs[i] = jsonContent["idCams"][std::to_string(i).c_str()];
    }
    // Compute params
    std::string tmpStdString = jsonContent["RxComputeParametersFile"];
    infoJSON.filenameComputeParameters = Rx::CRxString(tmpStdString.c_str());
    // Sequence file
    infoJSON.loadFromRawFile = jsonContent["fromRawFile"];
    if (infoJSON.loadFromRawFile) {
        // Load seq file path
        infoJSON.filenameRawFiles = new Rx::CRxString[infoJSON.numCams];
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            tmpStdString = jsonContent["pathRawFile"][std::to_string(i).c_str()];
            infoJSON.filenameRawFiles[i] = Rx::CRxString(tmpStdString.c_str());
        }
    }
    //Calibration
    infoJSON.specificCal = jsonContent["specificCalibration"];
    if (infoJSON.specificCal) {
        // Load calib GUID
        infoJSON.GUIDsCalib = new Rx::CRxString[infoJSON.numCams];
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            tmpStdString = jsonContent["GUIDsForCalibration"][std::to_string(i).c_str()];
            infoJSON.GUIDsCalib[i] = Rx::CRxString(tmpStdString.c_str());
        }
    }
    if (!infoJSON.loadFromRawFile) {
        //Exposure of cameras
        infoJSON.exposureCams = new float[infoJSON.numCams];
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            infoJSON.exposureCams[i] = jsonContent["exposureCams"][std::to_string(i).c_str()];
        }
    }
    // Extrinsic params path
    infoJSON.extrinsicParamsPath = jsonContent["ExtrinsicParamsFile"];
    std::cout << "Json path read" << std::endl;
}
 
void RaytrixStreamer::initMutex() {
    loaderMtxs = new std::mutex[infoJSON.numCams];
    loaderCVs = new std::condition_variable[infoJSON.numCams];
    canLoadImgs = new bool[infoJSON.numCams];
    memset(canLoadImgs, true, infoJSON.numCams * sizeof(bool));
}
 
void RaytrixStreamer::initRxRayFile() {
    std::cout << "Read RayFile: " << std::endl;
    try {
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            std::cout << "Seq " << i << ": " << infoJSON.filenameRawFiles[i].ToCString() << std::endl;
            seqReaders[i].Open(infoJSON.filenameRawFiles[i]);
            seqReaders[i].ReadFrame(rayImages[i], true, false);
 
            Rx::CRxArrayString GUIDs;
            Rx::LFR::CCalibrationManager::GetCalibrationGUIDs(GUIDs);
            int idxInGUIDs = -1;
            for (size_t j = 0; infoJSON.specificCal && j < GUIDs.Length(); ++j) {
                if (GUIDs.GetDataPtr()[j] == infoJSON.GUIDsCalib[i]) {
                    std::cout << "Find metric calibration wanted: " << infoJSON.GUIDsCalib[i].ToCString() << std::endl;
                    idxInGUIDs = j;
                }
            }
 
            if (idxInGUIDs >= 0) {
                camCalibrations[i] = new Rx::LFR::CCalibration();
                Rx::LFR::CCalibrationManager::LoadCalibration(*(camCalibrations[i]), GUIDs.GetDataPtr()[idxInGUIDs], true);
            }
            else {
                camCalibrations[i] = (Rx::LFR::CCalibration*)&(rayImages[i].GetCalibration());
            }
        }
    }
    catch (Rx::CRxException e) {
        std::cout << "Impossible to read Rays file... Check error code: " << e.ToString().ToCString() << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
}
 
/*---------------------------------*/
 
/*------- Print fps(functions)*/
 
void RaytrixStreamer::addMsgToPrint(const char* title, long long int averageTime, float frameNumber)
{
    if (printMsg == NULL) {
        whereToAddMsg = 0;
        printMsg = (char*)malloc(sizeof(char) * SIZE_MSG);
        if (printMsg == NULL)
            return;
    }
 
    size_t size = std::strlen(title) + 6 + 6;
    sprintf(printMsg + whereToAddMsg, "%s: %f ...", title, (frameNumber) * 1000.f / averageTime);
    whereToAddMsg += size;
}
 
void RaytrixStreamer::printFPS() {
    std::cout << '\r' << printMsg << std::flush;
}
 
void RaytrixStreamer::resetPrintMsg() {
    memset(printMsg, '\0', sizeof(char) * SIZE_MSG);
    whereToAddMsg = 0;
}
/*----------------------------*/
 
/*--------- Getter*/
 
float RaytrixStreamer::getMaxDepth() {
    const float ZFar = 1.3f; // -> !!! MAGIC NUMBER !!! from AcquKirt -> not used here
    return ZFar;
}
 
float RaytrixStreamer::getMinDepth() {
    const float ZNear = 0.4f; // -> !!! MAGIC NUMBER !!! from AcquKirt -> not used here
    return ZNear;
}
 
HvtIntrinsicsPerspective RaytrixStreamer::getRxIntrinsicParams(size_t idxCam) {
    Rx::LFR::CParameters<Rx::LFR::Params::ECalib::ID>& paramsCal = camCalibrations[idxCam]->GetParams();
    double f, sizePixel;
    paramsCal.GetValue(Rx::LFR::Params::ECalib::MainLensThick_NominalFocalLengthMM, f);
    paramsCal.GetValue(Rx::LFR::Params::ECalib::Sensor_PhysicalPixelSizeMM, sizePixel);
 
    HvtIntrinsicsPerspective intrinsicParams;
    intrinsicParams.focalX = (float) f / (((float)widthDefault / (float)infoJSON.widthDepth) * sizePixel);
    intrinsicParams.focalY = (float) f / (((float)heightDefault / (float)infoJSON.heightDepth) * sizePixel);
    intrinsicParams.principlePointX = ((float)infoJSON.width) / 2.0f;
    intrinsicParams.principlePointY = ((float)infoJSON.height) / 2.0f;
 
    return intrinsicParams;
}
 
HvtExtrinsics RaytrixStreamer::getRxExtrinsicParams(size_t idxCam) {
    HvtPosition pos;
    HvtRotation rot;
    unsigned isExCalib;
    Rx::LFR::CParameters<Rx::LFR::Params::ECalib::ID>& paramsCal = camCalibrations[idxCam]->GetParams();
    paramsCal.GetValue(Rx::LFR::Params::ECalib::IsExtrinsicCalibrated, isExCalib);
    if (isExCalib != 1) {
        std::cerr << "Camera " << idxCam << " is not extrinsincly calibrated... Reconstruction must be wrong." << std::endl;
    }
    Rx::CRxArrayDouble trans;
    paramsCal.GetValue(Rx::LFR::Params::ECalib::Translation_Global_Sensor, trans);
    double* dataPtr = (double*)trans.GetPointer();
    size_t sizeData = trans.Length();
    bool checkExParams = true;
    if (sizeData < 1)
        checkExParams = false;
 
    std::ifstream jsonRead{ infoJSON.extrinsicParamsPath, std::ifstream::binary };
    if (!jsonRead.good()) {
        std::cout << "Impossible to read JSON extrinsic file, check path... Path given: " << infoJSON.extrinsicParamsPath << std::endl;
        throw HVT_ERROR_NOT_FOUND;
    }
    nlohmann::json jsonContent;
    jsonRead >> jsonContent;
 
    if (checkExParams) {
        pos.x = (float)dataPtr[2] + (double) jsonContent["cameras"][idxCam]["Position"][0]; // x is forward/backward
        pos.y = (float)dataPtr[0] + (double) jsonContent["cameras"][idxCam]["Position"][1]; // y is left/right
        pos.z = (float)dataPtr[1] + (double) jsonContent["cameras"][idxCam]["Position"][2]; // z is up/down
        referencePlaneToCameraPlane[idxCam] = pos.x;
        double data;
        paramsCal.GetValue(Rx::LFR::Params::ECalib::Rotation_Global_Sensor_Pitch, data);
        rot.pitch = (float)data + (double) jsonContent["cameras"][idxCam]["Rotation"][0];
        paramsCal.GetValue(Rx::LFR::Params::ECalib::Rotation_Global_Sensor_Roll, data);
        rot.roll = (float)data + (double) jsonContent["cameras"][idxCam]["Rotation"][1];
        paramsCal.GetValue(Rx::LFR::Params::ECalib::Rotation_Global_Sensor_Yaw, data);
        rot.yaw = (float)data + (double) jsonContent["cameras"][idxCam]["Rotation"][2];
    }
    else {
        pos.x = 0.0f;
        pos.y = 0.0f;
        pos.z = 0.0f;
        referencePlaneToCameraPlane[idxCam] = 1.0f;
        rot.pitch = 0.0f;
        rot.roll = 0.0f;
        rot.yaw = 0.0f;
    }
    jsonRead.close();
 
 
    HvtExtrinsics extrinsics;
    extrinsics.position = pos;
    extrinsics.rotation = rot;
    return extrinsics;
}
 
Rx::LFR::CImageQueue& RaytrixStreamer::getCamBuffer(size_t idxCam)
{
    return camBuffers[idxCam];
}
/*---------------*/
 
/*--------------- RaytrixDLL functions for RGBD acquire and refine tasks*/
 
void RaytrixStreamer::getRGBDFormats() {
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        try{
            imgFormatRGB[i] = xCudaComputes[i].GetImageFormat(idRGB);
        }
        catch (Rx::CRxException e) {
            std::cerr << "Impossible to get image format... Check error code: "<< e.ToString().ToCString() << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
        if (imgFormatRGB[i].m_iWidth != infoJSON.width || imgFormatRGB[i].m_iHeight != infoJSON.height) {
            std::cerr << "RGB image formats do not correspond..." << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
        try {
            imgFormatDepth[i] = xCudaComputes[i].GetImageFormat(idDepth);
        }
        catch (Rx::CRxException e) {
            std::cerr << "Impossible to get image format... Check error code: " << e.ToString().ToCString() << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
        if (imgFormatDepth[i].m_iWidth != infoJSON.widthDepth || imgFormatDepth[i].m_iHeight != infoJSON.heightDepth) {
            std::cerr << "Depth image formats do not correspond..." << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
 
        pitchInCudaRGB[i] = xCudaComputes[i].GetDevicePointerPitch(idRGB);
        pitchInCudaDepth[i] = xCudaComputes[i].GetDevicePointerPitch(idDepth);
 
        std::cout << "RGB cam/file " << i << " :" << imgFormatRGB[i].ToString().ToCString() << std::endl;
        std::cout << "Depth cam/file "<< i <<" :" << imgFormatDepth[i].ToString().ToCString() << std::endl;
    }
    isFormatLoaded = true;
}
 
void RaytrixStreamer::launchAsyncLoader(bool needToWaitThreadToFinish) {
    std::thread t;
    if (infoJSON.loadFromRawFile) {
        t = std::thread(&RaytrixStreamer::asyncLoaderImageFromSeqFiles, this);
    }
    else {
        t = std::thread(&RaytrixStreamer::asyncLoaderImage, this);
    }
    if (needToWaitThreadToFinish) t.join();
    else t.detach();
}
 
void RaytrixStreamer::asyncLoaderImageFromOneSeqFile(size_t idxCam) {
    try {
        // Upload the image as the new ray image of all further CUDA computations
        if (seqReaders[idxCam].GetFrameCount() == seqReaders[idxCam].GetLastFrameIndex() + 1) {
            seqReaders[idxCam].SetNextFrameIndex(0);
        }
        seqReaders[idxCam].ReadFrame(rayImages[idxCam], false);
 
        xCudaComputes[idxCam].UploadRawImage(rayImages[idxCam]);
    }
    catch (Rx::CRxException e) {
        std::cerr << "Impossible to read or upload ray image to the cuda compute ... Check error code: " << e.ToString().ToCString() << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
}
 
void RaytrixStreamer::asyncLoaderImageFromSeqFiles() {
    bool* tmpSync = &imgsLoaded;
    std::unique_lock<std::mutex> lckSync(syncMtx);
    syncCV.wait(lckSync, [tmpSync] {return !(*tmpSync); });
    std::thread* threads = new std::thread[infoJSON.numCams];
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        threads[i] = std::thread(&RaytrixStreamer::asyncLoaderImageFromOneSeqFile, this, i);
    }
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        threads[i].join();
    }
    imgsLoaded = true;
    lckSync.unlock();
    syncCV.notify_all();
    delete[] threads;
}
 
void RaytrixStreamer::asyncLoaderImage() {
    size_t idxRefCam = 0;
    bool loadImgSuccess = false;
 
    bool* tmpSync = &imgsLoaded;
    std::unique_lock<std::mutex> lckSync(syncMtx);
    syncCV.wait(lckSync, [tmpSync] {return !(*tmpSync); });
    while(!loadImgSuccess){
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            std::thread(&RaytrixStreamer::asyncLoaderImageOneCam, this, i).detach();
        }
        if (infoJSON.numCams == 1) {
            bool* tmp = canLoadImgs;
            std::unique_lock<std::mutex> lck(loaderMtxs[0]);
            loaderCVs[0].wait(lck, [tmp] {return !(*tmp); });
            canLoadImgs[0] = true;
            lck.unlock();
            loaderCVs[0].notify_all();
            break;
        }
 
        bool* tmpLoaderRefCam = canLoadImgs + idxRefCam;
        std::unique_lock<std::mutex> lckLoaderRefCam(loaderMtxs[idxRefCam]);
        loaderCVs[idxRefCam].wait(lckLoaderRefCam, [tmpLoaderRefCam] {return !(*tmpLoaderRefCam); });
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            if (i == idxRefCam) continue;
 
            bool* tmp = canLoadImgs+i;
            std::unique_lock<std::mutex> lckLoader(loaderMtxs[i]);
            loaderCVs[i].wait(lckLoader, [tmp] {return !(*tmp) ; });
 
            loadImgSuccess = (abs(capturedImages[i].GetTimestamp() - capturedImages[idxRefCam].GetTimestamp()) < MAX_TIME_BETWEEN_CAMS);
            
            canLoadImgs[i] = true;
            lckLoader.unlock();
            loaderCVs[i].notify_all();
        }
        canLoadImgs[idxRefCam] = true;
        lckLoaderRefCam.unlock();
        loaderCVs[idxRefCam].notify_all();
    }
    try{
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            xCudaComputes[i].UploadRawImage(capturedImages[i]);
        }
    }
    catch (Rx::CRxException e) {
        std::cerr << "Impossible to upload raw image to the cuda compute ... Check error code: " << e.ToString().ToCString() << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
    imgsLoaded = true;
    lckSync.unlock();
    syncCV.notify_all();
}
 
void RaytrixStreamer::asyncLoaderImageOneCam(size_t idxCam) {
    bool* tmp = canLoadImgs + idxCam;
    std::unique_lock<std::mutex> lck(loaderMtxs[idxCam]);
    loaderCVs[idxCam].wait(lck, [tmp] {return *tmp; });
    
    bool imgLoadSucess = false;
    while (!imgLoadSucess) {
        if (camCaptureMode == Rx::Interop::Runtime30::Camera::ETriggerMode::Software_SnapShot) {
            // Trigger the camera
            auto& cam = xCamServer.GetCamera(idxCam);
            cam.Trigger();
        }
 
        // Wait for the image buffer to be not empty
        if(!camBuffers[idxCam].WaitForNotEmpty(MAX_TIME_WAIT_BUFFER)) continue;
        
        // Try to move the captured image out of the buffer
        imgLoadSucess = camBuffers[idxCam].MoveOut(capturedImages[idxCam]);
    }
    canLoadImgs[idxCam] = false;
    lck.unlock();
    loaderCVs[idxCam].notify_all();
}
 
void RaytrixStreamer::acquireRGBD(){
    try {
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            if (!xCudaComputes[i].Compute_TotalFocus(idSpaceRGB)) {
                std::cerr << "Impossible to compute total focus image and depth ... Camera: " << i << std::endl;
                throw HVT_ERROR_UNKNOWN;
            }
            if (!xCudaComputes[i].Compute_Depth3D(idSpaceDepth3D[0], idSpaceDepth3D[1])) {
                std::cerr << "Impossible to compute depth ... Camera: " << i << std::endl;
                throw HVT_ERROR_UNKNOWN;
            }
        }
    }
    catch (Rx::CRxException e) {
        std::cerr << "Impossible to read or compute... Check error code: " << e.ToString().ToCString() << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
    if (!isFormatLoaded) {
        getRGBDFormats();
    }
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        switch (imgFormatRGB[i].m_eDataType) {
            case Rx::Interop::Runtime28::EDataType::UByte:
                imgUByteRGB[i] = (uchar*)xCudaComputes[i].GetImageDevicePointer(idRGB);
                break;
            case Rx::Interop::Runtime28::EDataType::UShort:
                imgRGB[i] = (unsigned short*)xCudaComputes[i].GetImageDevicePointer(idRGB);
                break;
            default:
                std::cerr << "Not known RGBA format... Check format: " << imgFormatRGB[i].ToString().ToCString() << std::endl;
                throw HVT_ERROR_UNKNOWN;
        }
 
        switch (imgFormatDepth[i].m_eDataType) {
            case Rx::Interop::Runtime28::EDataType::UShort:
                imgDepth[i] = (unsigned short*)xCudaComputes[i].GetImageDevicePointer(idDepth);
                break;
            case Rx::Interop::Runtime28::EDataType::Float:
                imgFloatDepth[i] = (float*)xCudaComputes[i].GetImageDevicePointer(idDepth);
                break;
            default:
                std::cerr << "Not known depth format... Check format: " << imgFormatDepth[i].ToString().ToCString() << std::endl;
                throw HVT_ERROR_UNKNOWN;
        }
        //gpuErrchk(cudaDeviceSynchronize());
    }
}
 
void RaytrixStreamer::refineRGBD() {
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        switch (imgFormatRGB[i].m_eDataType) {
        case Rx::Interop::Runtime28::EDataType::UByte:
            if (pitchInCudaRGB[i] != sizeof(uchar) * 4 * infoJSON.width) {
                removePitch<uchar>(imgUByteRGB[i], finalRGB[i], infoJSON.width, infoJSON.height, pitchInCudaRGB[i] / (sizeof(uchar) * 4), 4, cudaStreams[i]);
            }
            else {
                gpuErrchk(cudaMemcpyAsync(finalRGB[i], imgUByteRGB[i], sizeof(uchar) * 4 * infoJSON.width * infoJSON.height, cudaMemcpyDeviceToDevice, cudaStreams[i]));
            }
            break;
        case Rx::Interop::Runtime28::EDataType::UShort:
            uShort2uChar(imgRGB[i], finalRGB[i], infoJSON.width, infoJSON.height, pitchInCudaRGB[i] / (sizeof(USHORT) * 4), 4, cudaStreams[i]);
            break;
        default:
            std::cerr << "Not known RGBA format... Check format: " << imgFormatRGB[i].ToString().ToCString() << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
 
        switch (imgFormatDepth[i].m_eDataType) {
            case Rx::Interop::Runtime28::EDataType::UShort:
                break;
            case Rx::Interop::Runtime28::EDataType::Float:
                scaleAddDataArrayUChannel<float>(imgFloatDepth[i], finalDepth[i], infoJSON.widthDepth, infoJSON.heightDepth, pitchInCudaDepth[i] / (sizeof(float) * 4), 4, channelDepth, -1 / 1000.0f, -referencePlaneToCameraPlane[i], cudaStreams[i]);
                break;
            default:
                std::cerr << "Not known depth format... Check format: " << imgFormatDepth[i].ToString().ToCString() << std::endl;
                throw HVT_ERROR_UNKNOWN;
        }
        temporalConsistencyAdjustement<float>(prevFinalDepth[i], finalDepth[i], infoJSON.widthDepth, infoJSON.heightDepth, 5, 0.05, 0.5, cudaStreams[i]);
    }
}
 
/*----------------------------------------------------------------------*/
 
/*--------- Hovitron API functions*/
 
void RaytrixStreamer::uploadFrame(size_t streamId, ReadStream& stream)
{
    auto imageIndex = slotStreamingIndex;
 
    auto& dstColorSlot = stream.colorSlots.at(imageIndex);
    auto& dstDepthSlot = stream.depthSlots.at(imageIndex);
 
    //gpuErrchk(cudaEventSynchronize(cudaEvents[streamId]));
    copyColor(dstColorSlot.cuda_ptr_surf,finalRGB[streamId],stream.width_color,stream.height_color, cudaStreams[streamId]);
    copyDepth(dstDepthSlot.cuda_ptr_surf, finalDepth[streamId], stream.width_depth, stream.height_depth, cudaStreams[streamId]);
    gpuErrchk(cudaEventRecord(cudaEvents[streamId],cudaStreams[streamId]));
}
 
void RaytrixStreamer::streamingLoop() {
    gpuErrchk(cudaSetDevice(vulkanGPU));
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        if (!infoJSON.loadFromRawFile) {
            // Start cameras
            std::cout << "Start camera " << i << std::endl;
            xCamServer.GetCamera(i).Start(camCaptureMode);
        }
    }
 
    beginAvgStreaming = std::chrono::steady_clock::now();
    std::chrono::steady_clock::time_point begin;
 
    launchAsyncLoader(true);
 
    std::unique_lock<std::mutex> lck(syncMtx, std::defer_lock);
    bool* cond = &imgsLoaded;
    while (running) {
        begin = std::chrono::steady_clock::now();
 
        lck.lock();
        syncCV.wait(lck, [cond] {return *cond; });
 
        acquireRGBD();
 
        *cond = false;
        lck.unlock();
        syncCV.notify_all();
 
        launchAsyncLoader();
 
        refineRGBD();
 
        for (size_t i = 0; i < infoJSON.numCams; ++i) {
            uploadFrame(i, readStreams[i]);
 
            readStreams[i].streamedFrame = readStreams[i].frameIndex(begin - beginAvgStreaming);
        }
        // Commit
        swapStreamingToPending();
        numFrames++;
 
        endStreaming = std::chrono::steady_clock::now();
 
        if ((int)numFrames % 100 == 0) {
            addMsgToPrint("Streaming Loop fps", std::chrono::duration_cast<std::chrono::milliseconds>(endStreaming - beginAvgStreaming).count(), numFrames);
            printFPS();
            resetPrintMsg();
        }
    }
    std::this_thread::yield();
}
 
void RaytrixStreamer::swapStreamingToPending() {
    std::lock_guard<std::mutex> l(indicesMutex); // JS
    std::swap(slotPendingIndex, slotStreamingIndex);
    newDataInPending = true;
};
 
void RaytrixStreamer::swapPendingToReading() {
    std::lock_guard<std::mutex> l(indicesMutex);
    if (newDataInPending)
    {
        std::swap(slotPendingIndex, slotReadingIndex);
        newDataInPending = false;
        // printf("Swapped %d to reading\n", slotReadingIndex);
    }
};
/*--------------------------------*/
 
/*---------- Debug functions*/
 
template<typename T> void RaytrixStreamer::writeInFileWindow(T* dataCorrect, T* dataDebug, size_t width, size_t height, size_t sizeWindow, int numCam) {
    if (width / 2 + sizeWindow > width || height / 2 + sizeWindow > height) {
        std::cerr << "Specify a smaller window size..." << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
    if (dataCorrect == NULL || dataDebug == NULL) {
        std::cerr << "Specify allocated array ... " << std::endl;
        throw HVT_ERROR_UNKNOWN;
    }
 
    const char outCorrect[] = "../RaytrixStreamer/dataCorrect.txt";
    const char outDebug[] = "../RaytrixStreamer/dataDebug.txt";
 
    fs::path pathCorrect = outCorrect;
    std::ofstream correctDataStream{ pathCorrect, std::ofstream::out | std::ofstream::trunc };
    if (!correctDataStream.good()) {
        std::cerr << "Impossible to open data correct file, check path... Path given: " << pathCorrect << std::endl;
        throw HVT_ERROR_NOT_FOUND;
    }
    fs::path pathDebug = outDebug;
    std::ofstream debugDataStream{ pathDebug, std::ofstream::out | std::ofstream::trunc };
    if (!debugDataStream.good()) {
        std::cerr << "Impossible to open data debug file, check path... Path given: " << pathDebug << std::endl;
        throw HVT_ERROR_NOT_FOUND;
    }
 
    for (size_t i = 0; i < sizeWindow; ++i) {
        for (size_t j = 0; j < sizeWindow; ++j) {
            correctDataStream << dataCorrect[(width / 2) * ((height / 2) + i) + j] << ",";
            debugDataStream << dataDebug[(width / 2) * ((height / 2) + i) + j] << ",";
        }
        correctDataStream << std::endl;
        debugDataStream << std::endl;
    }
 
    correctDataStream.close();
    debugDataStream.close();
}
 
void RaytrixStreamer::debugFloatDepth() {
    Rx::CRxImage imgTmp;
    float* imgTmp2 = new float[infoJSON.widthDepth * infoJSON.heightDepth];
    int width, height;
    gpuErrchk(cudaDeviceSynchronize());
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        try {
            xCudaComputes[i].Download(Rx::LFR::EImage::DepthMap_View_Virtual, imgTmp);
            imgTmp.GetSize(width, height);
        }
        catch (Rx::CRxException e) {
            std::cerr << "Error while debugging depth... Check error code: " << e.ToString().ToCString() << std::endl;
            throw HVT_ERROR_UNKNOWN;
        }
        std::cout << "Downloaded img " << i << ": Width: " << width << " Height: " << height << " Nb of bytes: " << imgTmp.GetByteCount() << " Format: " << imgTmp.GetFormat().ToString().ToCString() << std::endl;
        gpuErrchk(cudaMemcpy(imgTmp2, finalDepth[i], infoJSON.widthDepth * infoJSON.heightDepth * sizeof(float), cudaMemcpyDeviceToHost));
        std::cout << "CUDA img " << i << ": Width: " << infoJSON.widthDepth << " Height : " << infoJSON.heightDepth << " Nb of bytes : " << infoJSON.widthDepth * infoJSON.heightDepth * sizeof(float) << " Format: " << imgFormatDepth[i].ToString().ToCString() << std::endl;
        gpuErrchk(cudaDeviceSynchronize());
        writeInFileWindow<float>((float*)imgTmp.GetDataPtr(), imgTmp2, infoJSON.widthDepth, infoJSON.heightDepth, 100, i);
    }
    delete[] imgTmp2;
}
/*--------------------------*/
 
/*----------- ReadStream function*/
 
int RaytrixStreamer::ReadStream::frameIndex(Clock::duration time) const
{
    return (time / framePeriod);
}
 
bool RaytrixStreamer::ReadStream::nextFrameReady(Clock::duration time) const
{
    return streamedFrame != frameIndex(time);
}
/*-------------------------------*/
 
/* 
 
    RaytrixStreamer class functions
 
 ------------------------------------ */
 
 
 
/* ---------------------------
 
    Functions called by the C entry points. Each Hovitron streamer dll has the same entry points, then we define what we need for the corresponding dll in the functions below. 
 
*/
 
/*-------------- Constructor/Destructor */
 
RaytrixStreamer::RaytrixStreamer(const uint8_t uuid[VK_UUID_SIZE])
{
    findVulkanGPU(uuid);
    readInfoFromJSON();
    initRxSDK();
    if (infoJSON.loadFromRawFile) {
        initRxRayFile();
    }
    else {
        initRxCams();
    }
    initRxCudaCompute();
    initStreamParameters();
    initArrayCuda();
    initMutex();
}
 
RaytrixStreamer::~RaytrixStreamer()
{
    std::cout << "---- Destroy RaytrixStreamer dll ----" << std::endl;
    if (synthesisSem != NULL)
        delete synthesisSem;
    for (size_t i = 0; i < infoJSON.numCams; ++i) {
        xCamServer.GetCamera(i).Close();
    }
    if (infoJSON.loadFromRawFile) {
        delete[] seqReaders;
        delete[] rayImages;
    }
    Rx::LFR::CLightFieldRuntime::End();
}
/*--------------------------------------*/
 
void RaytrixStreamer::enumerateStreamsParameters(uint32_t* streamsCount, HvtRGBDStreamParameters* parameters) const
{
    cudaSetDevice(vulkanGPU);
    if (!parameters)
    {
        *streamsCount = readStreams.size();
        return;
    }
 
    if (*streamsCount != readStreams.size())
    {
        throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
    }
 
    int i = 0;
    for (const auto& readStream : readStreams)
    {
        HvtRGBDStreamParameters* SP = new HvtRGBDStreamParameters;
        SP->colorResolution = { (uint32_t)readStream.width_color, (uint32_t)readStream.height_color };
        SP->depthResolution = { (uint32_t)readStream.width_depth, (uint32_t)readStream.height_depth };
        SP->nearDepth = readStream.anear;
        SP->farDepth = readStream.afar;
        SP->colorFormat = (HvtImageFormat)readStream.colorFormat;
        SP->depthFormat = (HvtImageFormat)readStream.depthFormat;
        SP->slotCount = numSlots;
        SP->projectionType = readStream.projectionType;
 
        parameters[i] = *SP;
        //snprintf(parameters->name, HVT_MAX_STREAM_NAME_LENGHT, "%s", deviceNames.at(i).c_str());
        ++i;
    }
}
 
void RaytrixStreamer::importStreamImages(const HvtStreamImagesExportInfo& exportInfos)
{
    cudaSetDevice(vulkanGPU);
    auto isDepth = (bool)exportInfos.depth;
    auto& stream = readStreams.at(exportInfos.streamIndex);
    //auto format = isDepth ? stream.depthFormat : stream.colorFormat;
    // auto size = isDepth ? stream.depthFrameSize : stream.colorFrameSize;
    auto& slots = isDepth ? stream.depthSlots : stream.colorSlots;
    auto mipLevels = 1;
 
    // std::cout << "Init memory buffers CUDA" << std::endl;
 
    if (exportInfos.imagesCount != numSlots)
    {
        throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
    }
 
    for (int i = 0; i < numSlots; ++i)
    {
        auto mem = exportInfos.pImages[i];
        // std::cout << "External Memory loop" << std::endl;
 
        cudaExternalMemory_t cudaExtMemImageBuffer;
        cudaMipmappedArray_t cudaMipmappedImageArray;
 
        cudaExternalMemoryHandleDesc cudaExtMemHandleDesc;
        memset(&cudaExtMemHandleDesc, 0, sizeof(cudaExtMemHandleDesc));
 
        cudaExtMemHandleDesc.type = cudaExternalMemoryHandleTypeOpaqueWin32;
        cudaExtMemHandleDesc.handle.win32.handle = (HANDLE)mem.handle;
        cudaExtMemHandleDesc.size = mem.size;
 
        gpuErrchk(cudaImportExternalMemory(&cudaExtMemImageBuffer, &cudaExtMemHandleDesc));
 
        cudaExternalMemoryMipmappedArrayDesc externalMemoryMipmappedArrayDesc;
        memset(&externalMemoryMipmappedArrayDesc, 0, sizeof(externalMemoryMipmappedArrayDesc));
 
        cudaExtent extent = isDepth ? make_cudaExtent(stream.width_depth, stream.height_depth, 0) : make_cudaExtent(stream.width_color, stream.height_color, 0);
        cudaChannelFormatDesc formatDesc;
        formatDesc.x = isDepth ? 32 : 8;
        formatDesc.y = isDepth ? 0 : 8;
        formatDesc.z = isDepth ? 0 : 8;
        formatDesc.w = isDepth ? 0 : 8;
        formatDesc.f = isDepth ? cudaChannelFormatKindFloat : cudaChannelFormatKindUnsigned;
 
        externalMemoryMipmappedArrayDesc.offset = 0;
        externalMemoryMipmappedArrayDesc.formatDesc = formatDesc;
        externalMemoryMipmappedArrayDesc.extent = extent;
        externalMemoryMipmappedArrayDesc.flags = 0;
        externalMemoryMipmappedArrayDesc.numLevels = mipLevels;
 
        gpuErrchk(cudaExternalMemoryGetMappedMipmappedArray(&cudaMipmappedImageArray, cudaExtMemImageBuffer, &externalMemoryMipmappedArrayDesc));
 
        cudaArray_t cudaMipLevelArray;
        cudaResourceDesc resourceDesc;
 
        gpuErrchk(cudaGetMipmappedArrayLevel(&cudaMipLevelArray, cudaMipmappedImageArray, 0));
        // cudaMemcpy2DArrayToArray(cudaMipLevelArray, 0, 0, cudaMipLevelArray, 0, 0, stream.width * sizeof(uchar4), stream.height, cudaMemcpyDeviceToDevice);
 
        memset(&resourceDesc, 0, sizeof(resourceDesc));
        resourceDesc.resType = cudaResourceTypeArray;
        resourceDesc.res.array.array = cudaMipLevelArray;
 
        cudaSurfaceObject_t surfaceObject;
        gpuErrchk(cudaCreateSurfaceObject(&surfaceObject, &resourceDesc));
 
        ImageSlot IS;
        IS.size = mem.size;
        IS.vk_handle = &mem.handle;
        IS.cuda_ptr_surf = surfaceObject;
 
        slots.at(i) = IS;
    }
    (isDepth ? stream.importedDepth : stream.importedColor) = true;
}
 
void RaytrixStreamer::importSemaphore(const HvtSemaphoreExportInfo& exportInfos)
{
    cudaSetDevice(vulkanGPU);
    if (exportInfos.semaphore == NULL)
    {
        synthesisSem->isValid = false;
        synthesisSem->sem = NULL;
        return;
    }
 
    cudaExternalSemaphoreHandleDesc externalSemaphoreHandleDesc = {};
 
    if (exportInfos.type & HvtSemaphoreType::HVT_SEMAPHORE_TYPE_OPAQUE_WIN32_BIT)
    {
        externalSemaphoreHandleDesc.type = cudaExternalSemaphoreHandleTypeOpaqueWin32;
    }
    else if (exportInfos.type & HvtSemaphoreType::HVT_SEMAPHORE_TYPE_OPAQUE_WIN32_KMT_BIT)
    {
        externalSemaphoreHandleDesc.type = cudaExternalSemaphoreHandleTypeOpaqueWin32Kmt;
    }
    else if (exportInfos.type & HvtSemaphoreType::HVT_SEMAPHORE_TYPE_SYNC_FD_BIT)
    {
        externalSemaphoreHandleDesc.type = cudaExternalSemaphoreHandleTypeOpaqueFd;
    }
 
    externalSemaphoreHandleDesc.handle.win32.handle = (HANDLE)exportInfos.semaphore;
    externalSemaphoreHandleDesc.flags = 0;
 
    cudaImportExternalSemaphore(&synthesisSem->sem, &externalSemaphoreHandleDesc);
    synthesisSem->isValid = true;
    // cudaWaitExternalSemaphoresAsync();
}
 
void RaytrixStreamer::destroySemaphore(Semaphore* sem) const
{
    cudaSetDevice(vulkanGPU);
    delete sem;
}
 
void RaytrixStreamer::startStreaming()
{
    //cudaSetDevice(vulkanGPU);
    // assert that we have everything ready for streaming
    for (auto& stream : readStreams)
    {
        if (!stream.importedColor || !stream.importedDepth)
        {
            throw HvtResult::HVT_ERROR_CALL_ORDER;
        }
    }
 
    // Start worker
    running = true;
    std::cout << "Starting streaming" << std::endl;
    streamingThread = std::thread([&]
        { streamingLoop(); });
}
 
void RaytrixStreamer::acquireStreamsFrames(const HvtAcquireStreamFramesInfo& infos)
{
    gpuErrchk(cudaSetDevice(vulkanGPU));
    if (infos.frameInfoCount != readStreams.size())
    {
        throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
    }
 
    swapPendingToReading();
 
    auto imageIndex = slotReadingIndex;
 
    for (uint32_t i = 0; i < infos.frameInfoCount; ++i)
    {
        gpuErrchk(cudaEventSynchronize(cudaEvents[i]));
 
        auto& stream = readStreams.at(i);
        auto& desc = infos.pStreamFrameInfos[i];
        desc.extrinsics = stream.extrinsics;
        desc.intrinsics = stream.intrinsics;
        desc.imageIndex = imageIndex;
    }
}
 
void RaytrixStreamer::releaseStreamsFrames(Semaphore* waitSem)
{
    std::cout << "release frames" << std::endl;
}
 
void RaytrixStreamer::stopStreaming()
{
    cudaSetDevice(vulkanGPU);
    running = false;
    if (streamingThread.joinable())
    {
        streamingThread.join();
    }
}
 
 
 
 
template <typename Closure>
HvtResult exceptionFirewall(Closure&& clos)
{
    try
    {
        clos();
    }
    catch (HvtResult res)
    {
        return res;
    }
    catch (const std::exception& e)
    {
        std::cerr << "Catched exception at C boundary : \"" << e.what() << "\"" << std::endl;
        return HvtResult::HVT_ERROR_UNKNOWN;
    }
    catch (...)
    {
        return HvtResult::HVT_ERROR_UNKNOWN;
    }
    return HvtResult::HVT_SUCESS;
}
 
template <typename T>
void checkNonNull(T ptr)
{
    if (!ptr)
    {
        throw HvtResult::HVT_ERROR_INVALID_HANDLE;
    }
}
 
 
extern "C"
{
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtCreateStreamingContext(const HvtStreamingContextCreateInfo* createInfo, HvtStreamingContext* outStreamingContext)
    {
        //std::cout << "hvtCreateStreamingContext" << std::endl;
        return exceptionFirewall([&]
            {
                checkNonNull(createInfo);
 
                if (createInfo->headerVersion != HVT_HEADER_VERSION) {
                    throw HvtResult::HVT_ERROR_HEADER_VERSION;
                }
 
                auto context = new RaytrixStreamer(createInfo->graphicsDeviceUUID);
                *outStreamingContext = context->to_handle(); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtEnumerateStreamsParameters(HvtStreamingContext streamingContext, uint32_t* pStreamParameterCount, HvtRGBDStreamParameters* pStreamParameters)
    {
        //std::cout << "hvtEnumerateStreamsParameters" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                checkNonNull(pStreamParameterCount);
                context->enumerateStreamsParameters(pStreamParameterCount, pStreamParameters); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportStreamImages(HvtStreamingContext streamingContext, const HvtStreamImagesExportInfo* exportInfos)
    {
        //std::cout << "hvtExportStreamImages" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                checkNonNull(exportInfos);
                context->importStreamImages(*exportInfos); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportSemaphore(HvtStreamingContext streamingContext, const HvtSemaphoreExportInfo* exportInfo, HvtSemaphore* outSemaphore)
    {
        //std::cout << "hvtExportSemaphore" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                checkNonNull(exportInfo);
                context->importSemaphore(*exportInfo); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroySemaphore(HvtStreamingContext streamingContext, HvtSemaphore semaphore)
    {
        //std::cout << "hvtDestroySemaphore" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                context->destroySemaphore(Semaphore::check(semaphore)); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStartStreaming(HvtStreamingContext streamingContext)
    {
        //std::cout << "hvtStartStreaming" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                context->startStreaming(); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtAcquireStreamsFrames(HvtStreamingContext streamingContext, const HvtAcquireStreamFramesInfo* infos)
    {
        // std::cout << "hvtAcquireStreamsFrames" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                context->acquireStreamsFrames(*infos); });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtReleaseStreamsFrames(HvtStreamingContext streamingContext, HvtSemaphore waitSemaphore)
    {
        // std::cout << "hvtReleaseStreamsFrames" << std::endl;
        return exceptionFirewall([&]
            {
                //auto context = AcqKiRT::check(streamingContext);
                // context->releaseStreamsFrames(Semaphore::opt_check(waitSemaphore)); 
            });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStopStreaming(HvtStreamingContext streamingContext)
    {
        //std::cout << "hvtStopStreaming" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                context->stopStreaming();
            });
    }
 
    HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroyStreamingContext(HvtStreamingContext streamingContext)
    {
        //std::cout << "hvtDestroyStreamingContext" << std::endl;
        return exceptionFirewall([&]
            {
                auto context = RaytrixStreamer::check(streamingContext);
                delete context; });
    }
}