opencvStreamer.cpp

/* ----------------------
* 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.
---------------------- */
 
 
#include "opencvStreamer.h"
 
#include <array>
#include <iostream>
 
#include"opencvReading.h"
 
using rvs::detail::ColorSpace;
using rvs::detail::g_color_space;
 
    VULKAN_HPP_DEFAULT_DISPATCH_LOADER_DYNAMIC_STORAGE;
 
    // Returns format and half bytes per pixels
    static std::pair<vk::Format, size_t> vkColorFormatFromParams(const rvs::Parameters& params, InternalFormatClass internalFormat) {
        if (internalFormat == InternalFormatClass::YUV) {
            switch (params.getColorBitDepth()) {
            case 16:
                return { vk::Format::eG16B16R163Plane420Unorm, 6 };
            case 10:
                return { vk::Format::eG10X6B10X6R10X63Plane420Unorm3Pack16, 6 };
            default:
                return { vk::Format::eG8B8R83Plane420Unorm, 3 };
            }
        }else {
            if (params.getColorBitDepth() > 8 && params.getColorBitDepth() < 16) {
                return { vk::Format::eR16G16B16A16Unorm, 6 };
            }
            else {
                return { vk::Format::eR8G8B8A8Unorm, 3 };
            }
        }
    }
 
    // Returns format and byte per pixels
    static std::pair<vk::Format, size_t> vkDepthFormatFromParams(const rvs::Parameters& params) {
        switch (params.getDepthBitDepth()) {
        case 32:
            return { vk::Format::eR32Sfloat, 3};
        case 16:
            return { vk::Format::eR16Unorm, 2 };
        case 10:
            //TODO find a better solution
            return { vk::Format::eR16Unorm, 2 };
        default:
            return { vk::Format::eR8Unorm, 1 };
        }
    }
 
    int opencvStreamer::ReadStream::frameIndex(Clock::duration time) const {
        return (time / framePeriod) % frameCount;
    }
 
    bool opencvStreamer::ReadStream::nextFrameReady(Clock::duration time) const {
 
        return streamedFrame != frameIndex(time);
    }
 
    opencvStreamer::opencvStreamer(std::span<const uint8_t, VK_UUID_SIZE> uuid)
    {
        initSettings();
        initVk(uuid);
        initVkResources();
    }
 
    void opencvStreamer::initVk(std::span<const uint8_t, VK_UUID_SIZE> uuid) {
        vk::DynamicLoader dl;
        VULKAN_HPP_DEFAULT_DISPATCHER.init(dl.getProcAddress<PFN_vkGetInstanceProcAddr>("vkGetInstanceProcAddr"));
 
        constexpr auto neededInstanceExts = std::array{
            VK_KHR_EXTERNAL_MEMORY_CAPABILITIES_EXTENSION_NAME,
            VK_KHR_EXTERNAL_SEMAPHORE_CAPABILITIES_EXTENSION_NAME,
        };
 
        vk::ApplicationInfo ainfos(
            "opencvStreamer", VK_MAKE_VERSION(0, 0, 1), "No Engine", VK_MAKE_VERSION(0, 0, 0), VK_API_VERSION_1_2
        );
 
        instance = vk::createInstanceUnique(
            vk::InstanceCreateInfo(
                {}, //Flags
                &ainfos,
                {},
                neededInstanceExts
            ));
 
        VULKAN_HPP_DEFAULT_DISPATCHER.init(*instance);
 
        constexpr auto neededDeviceExts = std::array{
          #ifdef WIN32
            VK_KHR_EXTERNAL_MEMORY_WIN32_EXTENSION_NAME,
            VK_KHR_EXTERNAL_SEMAPHORE_WIN32_EXTENSION_NAME,
          #else
            VK_KHR_EXTERNAL_MEMORY_FD_EXTENSION_NAME,
            VK_KHR_EXTERNAL_SEMAPHORE_FD_EXTENSION_NAME
            //no need for this
            #endif
        };
 
        auto phyDevs = instance->enumeratePhysicalDevices();
 
        auto it = std::find_if(phyDevs.begin(), phyDevs.end(), [&](const vk::PhysicalDevice& phy) {
            vk::PhysicalDeviceIDProperties idProps;
            vk::PhysicalDeviceProperties2 prop2;
            prop2.pNext = &idProps;
            phy.getProperties2(&prop2);
            return strncmp((const char*)idProps.deviceUUID.data(), (const char*)uuid.data(), VK_UUID_SIZE) == 0;
            });
 
        if (it == phyDevs.end()) {
            throw std::runtime_error("Could not find physical device corresponding to UUID");
        }
        phyDevice = *it; //TODO find device with right UUID
 
        float priority[] = { 1.f,1.0f };
 
        queueFamilyIndex = 0;
        auto queuesInfos = std::array{ vk::DeviceQueueCreateInfo({}, queueFamilyIndex, 2, priority) };
 
        device = phyDevice.createDeviceUnique(
            vk::DeviceCreateInfo(
                {}, //Flags
                queuesInfos, //Queues
                {}, //Layers
                neededDeviceExts
            ));
 
        VULKAN_HPP_DEFAULT_DISPATCHER.init(*device);
 
        queue = device->getQueue(queueFamilyIndex, 0);
        syncQueue = device->getQueue(queueFamilyIndex, 1);
    }
 
    template<typename Closure>
    void opencvStreamer::oneTimeSubmit(Closure&& func, vk::SubmitInfo submitInfo) {
        static thread_local vk::UniqueCommandPool commandPool;
        static thread_local vk::UniqueFence oneTimeFence;
 
        if (!commandPool) {
            commandPool = device->createCommandPoolUnique(
                vk::CommandPoolCreateInfo({}, queueFamilyIndex)); //TODO choose proper queueFamilyIndex
        }
 
        if (!oneTimeFence) {
            oneTimeFence = device->createFenceUnique(vk::FenceCreateInfo());
        }
 
        auto commandBuffers = device->allocateCommandBuffersUnique(
            vk::CommandBufferAllocateInfo(
                *commandPool,
                vk::CommandBufferLevel::ePrimary,
                1));
        auto& cmdbuf = *commandBuffers.front();
 
        cmdbuf.begin(vk::CommandBufferBeginInfo{});
        func(cmdbuf);
        cmdbuf.end();
 
        submitInfo.setCommandBuffers(cmdbuf);
 
        device->resetFences(*oneTimeFence);
        {
            std::scoped_lock l(queueMutex);
            queue.submit(submitInfo, *oneTimeFence);
        }
        (void)device->waitForFences(*oneTimeFence, VK_TRUE, UINT64_MAX);
    }
 
    void opencvStreamer::initSettings() {
        const char* jsonPath = getenv("HVT_JSON_PATH");
 
        if (!jsonPath) {
            std::cerr << "No HVT_JSON_PATH env variable set, cannot load settings" << std::endl;
            throw HvtResult::HVT_ERROR_NOT_FOUND;
        }
 
        auto path = std::filesystem::path(jsonPath);
        if (!std::filesystem::exists(path)) {
            std::cerr << "File " << path << " does not exist" << std::endl;
            throw HvtResult::HVT_ERROR_NOT_FOUND;
        }
 
        auto dir = std::filesystem::path(path).parent_path();
 
        config = rvs::Config::loadFromFile(path.filename().string(), dir.string());
 
        size_t numViews = config->InputCameraNames.size();
        if (config->params_real.size() != numViews ||
            config->depth_names.size() != numViews ||
            config->texture_names.size() != numViews) {
            throw std::runtime_error("File and settings size mismatch");
        }
 
        if (g_color_space == rvs::detail::ColorSpace::RGB) {
            internalFormat = InternalFormatClass::RGB;
        }
 
        auto frameRate = 3; //TODO read this from somewhere....
        using namespace std::chrono_literals;
        auto framePeriod = std::chrono::nanoseconds(1s) / frameRate;
 
        for (size_t i = 0; i < numViews; i++) {
            const auto& params = config->params_real.at(i);
            const auto& colorFileName = config->texture_names.at(i);
            const auto& depthFileName = config->depth_names.at(i);
 
            std::filesystem::path cp(colorFileName);
            auto colorFilePath = cp.is_absolute() ? cp : dir / cp;
 
            std::filesystem::path dp(depthFileName);
            auto depthFilePath = dp.is_absolute() ? dp : dir / dp;
 
 
            auto colorFile = std::ifstream(colorFilePath, std::ifstream::ate | std::ifstream::binary | std::ifstream::in);
            if (!colorFile.is_open()) {
                throw std::runtime_error(colorFileName); //TODO better error message
            }
 
            auto depthFile = std::ifstream(depthFilePath, std::ifstream::ate | std::ifstream::binary | std::ifstream::in);
            if (!depthFile.is_open()) {
                throw std::runtime_error(depthFileName);
            }
 
            auto [colorFormat, colorHalfBytesPerPixel] = vkColorFormatFromParams(params, internalFormat);
            auto [depthFormat, depthBytesPerPixel] = vkDepthFormatFromParams(params);
 
            size_t pixelCount = params.getSize().width * params.getSize().height;
 
            size_t colorSize = (pixelCount * colorHalfBytesPerPixel) / 2;
            size_t depthSize = pixelCount * depthBytesPerPixel;
 
            size_t depthStride = depthSize * 3 / 2;
            if (config->params_real[i].getDepthColorFormat() == rvs::ColorFormat::YUV400) {
                depthStride = depthSize;
            }
 
            auto colorFileSize = colorFile.tellg();
            auto depthFileSize = depthFile.tellg();
            auto frameCount = colorFileSize / colorSize;
            if (frameCount != (depthFileSize / depthStride)) {
                throw std::runtime_error("Mismatching depth and color framecount");
            }
 
 
 
            HvtIntrinsics intrinsics;
            HvtProjectionType ptype;
            if (params.getProjectionType() == "Perspective") {
                ptype = HvtProjectionType::HVT_PROJECTION_PERSPECTIVE;
                intrinsics.perspective = HvtIntrinsicsPerspective{
                  .focalX = params.getFocal()[0],
                  .focalY = params.getFocal()[1],
                  .principlePointX = params.getPrinciplePoint()[0],
                  .principlePointY = params.getPrinciplePoint()[1]
                };
            }
            else if (params.getProjectionType() == "Equirectangular") {
                ptype = HvtProjectionType::HVT_PROJECTION_EQUIRECTANGULAR;
                intrinsics.equirectangular = HvtIntrinsicsEquirectangular{
                  .verticalRange = {params.getVerRange()[0], params.getVerRange()[1]},
                  .horizontalRange = {params.getHorRange()[0], params.getHorRange()[1]},
                };
            }
            else {
                throw std::runtime_error("Unknown projection type " + params.getProjectionType());
            }
 
            HvtExtrinsics extrinsics = {
              .position = {params.getPosition()[0], params.getPosition()[1], params.getPosition()[2]},
              .rotation = {params.getRotation()[0], params.getRotation()[1], params.getRotation()[2]}
            };
 
            if (InternalFormatClass::RGB == internalFormat) {
                colorSize = 2*colorHalfBytesPerPixel * pixelCount * 4;
            }
 
            readStreams.push_back(
                ReadStream{
                  .colorFile = std::move(colorFile),
                  .depthFile = std::move(depthFile),
                  .colorFrameStride = colorSize,
                  .colorFrameSize = colorSize,
                  .depthFrameStride = depthStride,
                  .depthFrameSize = depthSize,
                  .resolution = {params.getSize().width, params.getSize().height},
                  .projectionType = ptype,
                  .intrinsics = intrinsics,
                  .extrinsics = extrinsics,
                  .anear = params.getDepthRange()[0],
                  .afar = params.getDepthRange()[1],
                  .colorFormat = colorFormat,
                  .depthFormat = depthFormat,
                  .framePeriod = framePeriod,
                  .frameCount = (int)frameCount,
                  .fileNameColor = colorFilePath.string(),
                  .fileNameDepth = depthFilePath.string()
                });
        }
    }
 
    void opencvStreamer::initVkResources() {
        for (ReadStream& stream : readStreams) {
            auto colorSize = stream.colorFrameSize;
            auto depthSize = stream.depthFrameSize;
            auto buffer = device->createBufferUnique(
                vk::BufferCreateInfo(
                    {}, //Flags,
                    colorSize + depthSize,
                    vk::BufferUsageFlagBits::eTransferSrc,
                    vk::SharingMode::eExclusive)
            );
 
            auto reqs = device->getBufferMemoryRequirements(*buffer);
            auto memIndex = findMemoryType(reqs.memoryTypeBits, vk::MemoryPropertyFlagBits::eHostCoherent | vk::MemoryPropertyFlagBits::eHostVisible);
 
            auto memory = device->allocateMemoryUnique(
                vk::MemoryAllocateInfo(reqs.size, memIndex));
 
            device->bindBufferMemory(*buffer, *memory, 0);
            auto mappedStagging = device->mapMemory(*memory, 0, colorSize + depthSize);
 
            stream.stagingBuffer = std::move(buffer);
            stream.stagingMemory = std::move(memory);
            stream.stagingMapping = (char*)mappedStagging;
        }
    }
 
    uint32_t opencvStreamer::findMemoryType(uint32_t typeFilter, vk::MemoryPropertyFlags properties)
    {
        vk::PhysicalDeviceMemoryProperties memProperties = phyDevice.getMemoryProperties();
        for (uint32_t i = 0; i < memProperties.memoryTypeCount; i++) {
            if ((typeFilter & (1 << i)) && (memProperties.memoryTypes[i].propertyFlags & properties) == properties) {
                return i;
            }
        }
        throw std::runtime_error("failed to find suitable memory type!");
    }
 
    void opencvStreamer::enumerateStreamsParameters(uint32_t* streamsCount, HvtRGBDStreamParameters* parameters) const {
        if (!parameters) {
            *streamsCount = readStreams.size();
            return;
        }
 
        if (*streamsCount != readStreams.size()) {
            throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
        }
 
        for (int i = 0; const auto & readStream : readStreams) {
            parameters[i] = HvtRGBDStreamParameters{
              .colorResolution = {(uint32_t)readStream.resolution.x, (uint32_t)readStream.resolution.y},
              .depthResolution = {(uint32_t)readStream.resolution.x, (uint32_t)readStream.resolution.y},
 
              .nearDepth = readStream.anear,
              .farDepth = readStream.afar,
 
              .colorFormat = (HvtImageFormat)readStream.colorFormat,
              .depthFormat = (HvtImageFormat)readStream.depthFormat,
 
              .slotCount = numSlots,
              .projectionType = readStream.projectionType
            };
            snprintf(parameters->name, HVT_MAX_STREAM_NAME_LENGHT, "%s", config->InputCameraNames.at(i).c_str());
            i++;
        }
    }
 
    void opencvStreamer::importStreamImages(const HvtStreamImagesExportInfo& exportInfos) {
        auto memtype = (vk::ExternalMemoryHandleTypeFlagBits)exportInfos.memoryType;
 
#ifdef WIN32
        if (memtype != vk::ExternalMemoryHandleTypeFlagBits::eOpaqueWin32)
#else 
        if (memtype != vk::ExternalMemoryHandleTypeFlagBits::eOpaqueFd)
#endif
        {
            throw HvtResult::HVT_ERROR_UNSUPPORTED_MEMORY_TYPE;
        }
 
        auto isDepth = (bool)exportInfos.depth;
        auto& stream = readStreams.at(exportInfos.streamIndex);
        auto format = isDepth ? stream.depthFormat : stream.colorFormat;
        auto& slots = isDepth ? stream.depthSlots : stream.colorSlots;
 
        if (exportInfos.imagesCount != numSlots) {
            throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
        }
 
        vk::ImageCreateInfo imgCreateInfo(
            {}, //Flags,
            vk::ImageType::e2D,
            format,
            vk::Extent3D(stream.resolution.x, stream.resolution.y, 1),
            1, //Mip count,
            1, //Array layers,
            vk::SampleCountFlagBits::e1,
            vk::ImageTiling::eOptimal,
            vk::ImageUsageFlagBits::eTransferDst,
            vk::SharingMode::eExclusive,
            {}, //Queue families
            vk::ImageLayout::eUndefined
        );
 
        for (int i = 0; i < numSlots; i++) {
            auto memoryInfos = exportInfos.pImages[i];
 
            auto imgImportInfo = vk::ExternalMemoryImageCreateInfo(memtype);
            auto createChain = vk::StructureChain(imgCreateInfo, imgImportInfo);
 
            auto image = device->createImageUnique(createChain.get<vk::ImageCreateInfo>());
            auto reqs = device->getImageMemoryRequirements(*image);
 
            auto memIndex = findMemoryType(reqs.memoryTypeBits, vk::MemoryPropertyFlagBits::eDeviceLocal);
 
            assert(reqs.size == memoryInfos.size);
            assert(reqs.alignment == memoryInfos.alignment);
 
            auto memoryAllocInfo = vk::StructureChain(
                vk::MemoryAllocateInfo(reqs.size, memIndex),
#ifdef WIN32
                vk::ImportMemoryWin32HandleInfoKHR(memtype, memoryInfos.handle)
#else
                vk::ImportMemoryFdInfoKHR(memtype, memoryInfos.handle)
#endif
            );
 
            auto memory = device->allocateMemoryUnique(memoryAllocInfo.get<vk::MemoryAllocateInfo>());
 
            device->bindImageMemory(*image, *memory, 0);
 
            // Transition images to their unique layout
            oneTimeSubmit([&](vk::CommandBuffer& cmd) {
                vk::ImageMemoryBarrier colorMemoryBarrier(
                    {},
                    vk::AccessFlagBits::eTransferWrite,
                    vk::ImageLayout::eUndefined, //Don't care about what was there previously
                    vk::ImageLayout::eTransferDstOptimal,
                    VK_QUEUE_FAMILY_IGNORED, // No queue ownership transfer
                    VK_QUEUE_FAMILY_IGNORED,
                    *image,
                    vk::ImageSubresourceRange(
                        vk::ImageAspectFlagBits::ePlane0 | vk::ImageAspectFlagBits::ePlane1 | vk::ImageAspectFlagBits::ePlane2,
                        0, 1,
                        0, 1));
 
                vk::ImageMemoryBarrier depthMemoryBarrier(
                    {},
                    vk::AccessFlagBits::eTransferWrite,
                    vk::ImageLayout::eUndefined,
                    vk::ImageLayout::eTransferDstOptimal,
                    VK_QUEUE_FAMILY_IGNORED,
                    VK_QUEUE_FAMILY_IGNORED,
                    *image,
                    vk::ImageSubresourceRange(
                        vk::ImageAspectFlagBits::eColor,
                        0, 1,
                        0, 1));
 
                cmd.pipelineBarrier(
                    vk::PipelineStageFlagBits::eTransfer,
                    vk::PipelineStageFlagBits::eTransfer,
                    vk::DependencyFlagBits::eByRegion,
                    {},
                    {},
                    isDepth ? depthMemoryBarrier : colorMemoryBarrier);
                });
 
            slots.at(i) = ImageSlot{
              .image = std::move(image),
              .memory = std::move(memory)
            };
        }
        (isDepth ? stream.importedDepth : stream.importedColor) = true;
    }
 
    Semaphore* opencvStreamer::importSemaphore(const HvtSemaphoreExportInfo& exportInfos) {
#ifdef WIN32
        if ((vk::ExternalSemaphoreHandleTypeFlagBits)exportInfos.type != vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueWin32) {
#else
        if ((vk::ExternalSemaphoreHandleTypeFlagBits)exportInfos.type != vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueFd) {
#endif
            throw HvtResult::HVT_ERROR_UNSUPPORTED_SEMAPHORE_TYPE;
        }
 
        auto sem = device->createSemaphoreUnique({});
#ifdef WIN32
        device->importSemaphoreWin32HandleKHR(
            vk::ImportSemaphoreWin32HandleInfoKHR(
                *sem,
                {},
                vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueWin32,
                exportInfos.semaphore
            ));
#else
        device->importSemaphoreFdKHR(
            vk::ImportSemaphoreFdInfoKHR(
                *sem,
                {},
                vk::ExternalSemaphoreHandleTypeFlagBits::eOpaqueFd,
                exportInfos.semaphore
            ));
#endif
        return new Semaphore{
          .sem = std::move(sem)
        };
    }
 
    void opencvStreamer::destroySemaphore(Semaphore * sem) const {
        delete sem;
    }
 
    void opencvStreamer::startStreaming() {
        //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;
        streamingThread = std::thread([&] {
            streamingLoop();
            });
    }
 
    void opencvStreamer::acquireStreamsFrames(const HvtAcquireStreamFramesInfo & infos) {
        if (infos.frameInfoCount != readStreams.size()) {
            throw HvtResult::HVT_ERROR_WRONG_BUFFER_SIZE;
        }
 
        swapPendingToReading();
 
        auto imageIndex = slotReadingIndex;
 
        for (int i = 0; i < infos.frameInfoCount; i++) {
            auto& stream = readStreams.at(i);
            auto& desc = infos.pStreamFrameInfos[i];
            desc.extrinsics = stream.extrinsics;
            desc.intrinsics = stream.intrinsics;
            desc.imageIndex = imageIndex;
        }
 
        if (infos.signalSemaphore) {
            auto sem = Semaphore::check(infos.signalSemaphore);
            std::scoped_lock l(queueMutex);
            syncQueue.submit(
                vk::SubmitInfo(
                    {},
                    {},
                    {},
                    *sem->sem));
        }
    }
 
    void opencvStreamer::releaseStreamsFrames(Semaphore * waitSem) {
        std::scoped_lock l(queueMutex);
 
        if (waitSem) {
            vk::PipelineStageFlags stage = vk::PipelineStageFlagBits::eTopOfPipe;
            syncQueue.submit(
                vk::SubmitInfo(
                    *waitSem->sem,
                    stage,
                    {},
                    {}));
        }
    }
 
    void opencvStreamer::stopStreaming() {
        running = false;
        if (streamingThread.joinable()) {
            streamingThread.join();
        }
    }
 
    opencvStreamer::~opencvStreamer() {
        stopStreaming();
    }
 
    void opencvStreamer::streamingLoop() {
 
        auto startTime = Clock::now();
 
        while (running) {
 
            auto now = Clock::now();
            auto time = now - startTime;
 
            int readyCount = 0;
            for (auto& stream : readStreams) {
                if (stream.nextFrameReady(time)) {
                    readyCount++;
                }
            }
 
            if (readyCount == readStreams.size()) {
 
                oneTimeSubmit([&](vk::CommandBuffer& cmd) {
                    for (int i = 0; auto & stream : readStreams) {
                        if (stream.nextFrameReady(time)) {
                            uploadFrame(cmd, i, stream, stream.frameIndex(time));
                            stream.streamedFrame = stream.frameIndex(time);
                        }
                        i++;
                    }
                    });
 
                // Commit
                swapStreamingToPending();
            }
            std::this_thread::yield();
        }
    }
 
    void opencvStreamer::uploadFrame(vk::CommandBuffer cmd, int streamId, ReadStream & stream, int frame) {
 
        auto imageIndex = slotStreamingIndex;
 
        auto& dstColorSlot = stream.colorSlots.at(imageIndex);
        auto& dstDepthSlot = stream.depthSlots.at(imageIndex);
 
        if (config->params_real.at(streamId).getDepthBitDepth() != 10) {
 
            auto depthFileOffset = frame * stream.depthFrameStride;
            stream.depthFile.seekg(depthFileOffset, std::ios_base::beg);
            stream.depthFile.read(stream.stagingMapping + stream.colorFrameSize, stream.depthFrameSize);
 
            if (!stream.depthFile) {
                std::cerr << "Failed to read more than " << stream.depthFile.gcount() << " bytes of data" << std::endl;
            }
 
            //Copy to depth
            cmd.copyBufferToImage(
                *stream.stagingBuffer,
                *dstDepthSlot.image,
                vk::ImageLayout::eTransferDstOptimal,
                { vk::BufferImageCopy(
                 stream.colorFrameSize,
                 stream.resolution.x,
                 stream.resolution.y,
                 vk::ImageSubresourceLayers(
                  vk::ImageAspectFlagBits::eColor,
                  0, 0, 1
                 ),
                 vk::Offset3D(0,0,0),
                 vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
                 ) });
 
        }
        else {
            cv::Mat depth = read_depth(stream.fileNameDepth, frame, config->params_real.at(streamId));
            auto imageIndex = slotStreamingIndex;
 
            //auto mainStride = (stream.depthFrameSize * 2) / 3;
            //auto secondStride = mainStride / 4;
            //auto half_res = stream.resolution / 2;
 
            void* data;
            VkDeviceSize imageSize = depth.total() * depth.elemSize();
            device->mapMemory(*stream.stagingMemory, stream.colorFrameSize, imageSize, {}, &data);
            memcpy(data, depth.data, imageSize);
            device->unmapMemory(*stream.stagingMemory);
 
            //Copy to color //TODO CHECK YUV PLANES
            cmd.copyBufferToImage(
                *stream.stagingBuffer,
                *dstDepthSlot.image,
                vk::ImageLayout::eTransferDstOptimal, { vk::BufferImageCopy(
                    stream.colorFrameSize,
                    stream.resolution.x,
                    stream.resolution.y,
                    vk::ImageSubresourceLayers(
                      vk::ImageAspectFlagBits::eColor,
                      0, 0, 1
                    ),
                    vk::Offset3D(0,0,0),
                    vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
                  ) });
        }
 
 
        if (InternalFormatClass::YUV == internalFormat) {
            auto colorFileOffset = frame * stream.colorFrameStride;
            
 
            stream.colorFile.seekg(colorFileOffset, std::ios_base::beg);
            
 
            // Those call might potentially block a lot on slow hdds....
            stream.colorFile.read(stream.stagingMapping, stream.colorFrameSize);
            
 
            if (!stream.colorFile) {
                std::cerr << "Failed to read more than " << stream.colorFile.gcount() << " bytes of data" << std::endl;
            }
 
        {
            auto mainStride = (stream.colorFrameSize * 2) / 3;
            auto secondStride = mainStride / 4;
            auto half_res = stream.resolution / 2;
 
            //Copy to color //TODO CHECK YUV PLANES
            cmd.copyBufferToImage(
                *stream.stagingBuffer,
                *dstColorSlot.image,
                vk::ImageLayout::eTransferDstOptimal,
                {
                  vk::BufferImageCopy(
                    0,
                    stream.resolution.x,
                    stream.resolution.y,
                    vk::ImageSubresourceLayers(
                      vk::ImageAspectFlagBits::ePlane0,
                      0, 0, 1
                    ),
                    vk::Offset3D(0,0,0),
                    vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
                  ),
                  vk::BufferImageCopy(
                    mainStride,
                    half_res.x,
                    half_res.y,
                    vk::ImageSubresourceLayers(
                      vk::ImageAspectFlagBits::ePlane1,
                      0, 0, 1
                    ),
                    vk::Offset3D(0,0,0),
                    vk::Extent3D(half_res.x, half_res.y, 1)
                  ),
                  vk::BufferImageCopy(
                    mainStride + secondStride,
                    half_res.x,
                    half_res.y,
                    vk::ImageSubresourceLayers(
                      vk::ImageAspectFlagBits::ePlane2,
                      0, 0, 1
                    ),
                    vk::Offset3D(0,0,0),
                    vk::Extent3D(half_res.x, half_res.y, 1)
                  )
                });
        }
        }
        else {
            cv::Mat color;
            color = read_color(stream.fileNameColor, frame, config->params_real.at(streamId));
            auto imageIndex = slotStreamingIndex;
 
            //auto mainStride = (stream.colorFrameSize * 2) / 3;
            //auto secondStride = mainStride / 4;
            //auto half_res = stream.resolution / 2;
 
            void* data;
            VkDeviceSize imageSize = color.total() * color.elemSize();
            device->mapMemory(*stream.stagingMemory, 0, imageSize, {}, &data);
            memcpy(data, color.data, imageSize);
            device->unmapMemory(*stream.stagingMemory);
 
            //Copy to color //TODO CHECK YUV PLANES
            cmd.copyBufferToImage(
                *stream.stagingBuffer,
                *dstColorSlot.image,
                vk::ImageLayout::eTransferDstOptimal, { vk::BufferImageCopy(
                    0,
                    stream.resolution.x,
                    stream.resolution.y,
                    vk::ImageSubresourceLayers(
                      vk::ImageAspectFlagBits::eColor,
                      0, 0, 1
                    ),
                    vk::Offset3D(0,0,0),
                    vk::Extent3D(stream.resolution.x, stream.resolution.y, 1)
                  ) });
        }
    }
 
    void opencvStreamer::swapStreamingToPending() {
        std::scoped_lock l(indicesMutex);
        std::swap(slotPendingIndex, slotStreamingIndex);
        newDataInPending = true;
        //printf("Swapped %d to pending\n", slotPendingIndex);
    }
 
    void opencvStreamer::swapPendingToReading() {
        std::scoped_lock l(indicesMutex);
        if (newDataInPending) {
            std::swap(slotPendingIndex, slotReadingIndex);
            newDataInPending = false;
            //printf("Swapped %d to reading\n", slotReadingIndex);
        }
    }
 
 
    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) {
            return exceptionFirewall([&] {
                checkNonNull(createInfo);
 
                if (createInfo->headerVersion != HVT_HEADER_VERSION) {
                    throw HvtResult::HVT_ERROR_HEADER_VERSION;
                }
 
                auto context = new opencvStreamer(createInfo->graphicsDeviceUUID);
                *outStreamingContext = context->to_handle();
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtEnumerateStreamsParameters(HvtStreamingContext streamingContext, uint32_t* pStreamParameterCount, HvtRGBDStreamParameters* pStreamParameters) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                checkNonNull(pStreamParameterCount);
                context->enumerateStreamsParameters(pStreamParameterCount, pStreamParameters);
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportStreamImages(HvtStreamingContext streamingContext, const HvtStreamImagesExportInfo* exportInfos) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                checkNonNull(exportInfos);
                context->importStreamImages(*exportInfos);
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtExportSemaphore(HvtStreamingContext streamingContext, const HvtSemaphoreExportInfo* exportInfo, HvtSemaphore* outSemaphore) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                checkNonNull(exportInfo);
                auto sem = context->importSemaphore(*exportInfo);
                *outSemaphore = sem->to_handle();
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroySemaphore(HvtStreamingContext streamingContext, HvtSemaphore semaphore) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                context->destroySemaphore(Semaphore::check(semaphore));
                });
        }
 
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStartStreaming(HvtStreamingContext streamingContext) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                context->startStreaming();
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtAcquireStreamsFrames(HvtStreamingContext streamingContext, const HvtAcquireStreamFramesInfo* infos) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                context->acquireStreamsFrames(*infos);
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtReleaseStreamsFrames(HvtStreamingContext streamingContext, HvtSemaphore waitSemaphore) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                context->releaseStreamsFrames(Semaphore::opt_check(waitSemaphore));
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtStopStreaming(HvtStreamingContext streamingContext) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                context->stopStreaming();
                });
        }
 
        HVTAPI_ATTR HvtResult HVTAPI_CALL hvtDestroyStreamingContext(HvtStreamingContext streamingContext) {
            return exceptionFirewall([&] {
                auto context = opencvStreamer::check(streamingContext);
                delete context;
                });
        }
 
    }