DynamicInputProvider.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>
 
* 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 "DynamicInputProvider.h"
#include "VulkanHelperFunction.h"
 
#include <map>
#include <filesystem>
 
#ifdef WIN32
#include <libloaderapi.h>
#else
#include <errno.h>
#endif
 
#ifdef __ANDROID__
#include <dlfcn.h>
#endif
static InputProvider::ProjectionType projectionHvtToOur(HvtProjectionType ptype) {
  switch(ptype) {
    case HvtProjectionType::HVT_PROJECTION_EQUIRECTANGULAR:
      return InputProvider::ProjectionType::PROJECTION_EQUIRECTANGULAR;
    case HvtProjectionType::HVT_PROJECTION_PERSPECTIVE:
      return InputProvider::ProjectionType::PROJECTION_PERSPECTIVE;
    default:
      return InputProvider::ProjectionType::PROJECTION_INVALID;
  }
}
 
static bool isFormatYUV(vk::Format format) {
  return format >= vk::Format::eG8B8G8R8422Unorm && format <= vk::Format::eG16B16R163Plane444Unorm;
}
 
DynamicInputProvider::DynamicInputProvider(const std::string& libraryPath, VulkanWrapper& vkw) : vkw(vkw)
{
  loadLibrary(libraryPath);
  init();
}
 
void DynamicInputProvider::loadLibrary(const std::string& libraryPath) {
  std::filesystem::path libPath = libraryPath;
  libPath = std::filesystem::absolute(libPath);
  if(!std::filesystem::exists(libPath)) {
    throw std::runtime_error("Could not load dynamic library " + libPath.string() + " : file does not exist.");
  }
  
  #ifdef WIN32
  SetErrorMode(0);
  auto module = LoadLibraryEx(libPath.string().c_str(), NULL, LOAD_LIBRARY_SEARCH_DEFAULT_DIRS | LOAD_LIBRARY_SEARCH_DLL_LOAD_DIR);
  #else
  //#error TODO implement on posix-like systems
 
  auto module = dlopen(libPath.string().c_str(),RTLD_LAZY);
 #endif
 
  if(module == NULL){
    #ifdef WIN32
      auto err = GetLastError();
      auto message = std::to_string(err);
      #else
      auto err = errno;
      auto message = strerror(err);
      #endif
    throw std::runtime_error("Could not load dynamic library " + libPath.string() + " ( " + message + " )");
  }
 
  auto fill_proc = [&]<typename Ptr>(Ptr& ptr, const char* name){
    #ifdef WIN32
    auto addr = GetProcAddress(module, name);
    #else
    auto addr = dlsym(module, name);
    #endif
    if(!addr) {
      #ifdef WIN32
      auto err = GetLastError();
      auto message = std::to_string(err);
      #else
      auto err = errno;
      auto message = strerror(err);
      #endif
      throw std::runtime_error(std::string("Failed to query entry point ") + name + " from module " + libraryPath + " ( " + message + " )");
    }
    ptr = reinterpret_cast<Ptr>(addr);
  };
 
  fill_proc(hvt.createStreamingContext, "hvtCreateStreamingContext");
  fill_proc(hvt.enumerateStreamsParameters, "hvtEnumerateStreamsParameters");
  fill_proc(hvt.exportStreamImages, "hvtExportStreamImages");
  fill_proc(hvt.exportSemaphore, "hvtExportSemaphore");
  fill_proc(hvt.acquireStreamsFrames, "hvtAcquireStreamsFrames");
  fill_proc(hvt.releaseStreamsFrames, "hvtReleaseStreamsFrames");
  fill_proc(hvt.startStreaming, "hvtStartStreaming");
  fill_proc(hvt.stopStreaming, "hvtStopStreaming");
  fill_proc(hvt.destroySemaphore, "hvtDestroySemaphore");
  fill_proc(hvt.destroyStreamingContext, "hvtDestroyStreamingContext");
 
 
}
 
class HvtError : public std::exception {
  public:
 
    HvtError(HvtResult code) : code(code) {}
    const char* what() const noexcept override{
      auto it = err_map.find(code);
      if(it == err_map.end()) {
        return "INVALID_ERROR";
      }
      return it->second;
    }
    HvtResult code;
  private:
    static inline const auto err_map = std::map<HvtResult, const char*>{
      {HVT_SUCESS, "HVT_SUCESS"},
      {HVT_RESULT_TIMEOUT, "HVT_RESULT_TIMEOUT"},
      {HVT_ERROR_UNKNOWN, "HVT_ERROR_UNKNOWN"} ,
      {HVT_ERROR_INVALID_HANDLE, "HVT_ERROR_INVALID_HANDLE"},
      {HVT_ERROR_WRONG_BUFFER_SIZE, "HVT_ERROR_WRONG_BUFFER_SIZE"},
      {HVT_ERROR_NOT_FOUND, "HVT_ERROR_NOT_FOUND"},
      {HVT_ERROR_UNSUPPORTED_FORMAT, "HVT_ERROR_UNSUPPORTED_FORMAT"},
      {HVT_ERROR_UNSUPPORTED_MEMORY_TYPE, "HVT_ERROR_UNSUPPORTED_MEMORY_TYPE"},
      {HVT_ERROR_UNSUPPORTED_SEMAPHORE_TYPE, "HVT_ERROR_UNSUPPORTED_SEMAPHORE_TYPE"},
      {HVT_ERROR_HEADER_VERSION, "HVT_ERROR_HEADER_VERSION"},
      {HVT_ERROR_CALL_ORDER, "HVT_ERROR_CALL_ORDER"}
    };
};
 
template<typename R, typename ...Params, typename ...Args>
static inline auto hvtCheck(R(*func)(Params...), Args&&... args) -> R {
  auto err = func(args...);
  if(err < 0){
    throw HvtError(err);
  }
  return err;
}
 
void DynamicInputProvider::init() {  
  vk::PhysicalDeviceIDProperties idProps;
  vk::PhysicalDeviceProperties2 prop2;
  prop2.pNext = &idProps;
 
  vkw.context.physicalDevice.getProperties2(&prop2);
 
  // Init streaming context
  HvtStreamingContextCreateInfo cinfos{
    .headerVersion = HVT_HEADER_VERSION
  };
 
  //Add our device UUID here
  std::copy(idProps.deviceUUID.begin(), idProps.deviceUUID.end(), cinfos.graphicsDeviceUUID);
 
  hvtCheck(hvt.createStreamingContext, &cinfos, &context);
 
  commandPool = vkw.context.device.createCommandPoolUnique(
        vk::CommandPoolCreateInfo({}, vkw.context.queueFamilyIndex));
  initStreams();
  initSynchronisation();
 
  hvtCheck(hvt.startStreaming, context);
}
 
void DynamicInputProvider::initStreams() {
  // Do two-call to get the list from the module
  uint32_t size = 0;
  hvtCheck(hvt.enumerateStreamsParameters, context, &size, nullptr);
 
  std::vector<HvtRGBDStreamParameters> params(size);
  hvtCheck(hvt.enumerateStreamsParameters, context, &size, params.data());
 
  // Convert the list to our format
  for(const auto& param : params) {
    cachedStreamParameters.push_back(
          StreamParameters{
            .streamName = std::string(param.name),
            .colorResolution = {param.colorResolution.width, param.colorResolution.height},
            .depthResolution = {param.depthResolution.width, param.depthResolution.height},
            .nearDepth = param.nearDepth,
            .farDepth = param.farDepth,
            .colorFormat = (vk::Format)param.colorFormat,
            .depthFormat = (vk::Format)param.depthFormat,
            .projectionType = projectionHvtToOur(param.projectionType)
          });
  }
 
  // Allocate the images storages
  auto dev = vkw.context.device;
 
  for(int i = 0; const auto& param : params){
    auto& sparam = cachedStreamParameters.at(i);
    auto colorRes = sparam.colorResolution;
    auto depthRes = sparam.depthResolution;
 
    Stream stream;
 
    bool yuv = isFormatYUV(sparam.colorFormat);
    if(yuv) {
      stream.ycbcrConversion = vkw.context.device.createSamplerYcbcrConversionUnique(
            vk::SamplerYcbcrConversionCreateInfo(
              sparam.colorFormat,
              vk::SamplerYcbcrModelConversion::eYcbcr709,
              vk::SamplerYcbcrRange::eItuFull,
              vk::ComponentMapping(),
              vk::ChromaLocation::eMidpoint,
              vk::ChromaLocation::eMidpoint,
              vk::Filter::eLinear,
              VK_FALSE
              )
            );
 
      stream.colorSampler = vkw.context.device.createSamplerUnique(
              vk::StructureChain(vk::SamplerCreateInfo(
                {},
                vk::Filter::eLinear,
                vk::Filter::eLinear,
                vk::SamplerMipmapMode::eNearest,
                vk::SamplerAddressMode::eClampToEdge,
                vk::SamplerAddressMode::eClampToEdge,
                vk::SamplerAddressMode::eClampToEdge)
              .setBorderColor(vk::BorderColor::eFloatOpaqueBlack),
               vk::SamplerYcbcrConversionInfo(
                  *stream.ycbcrConversion
                 )).get<vk::SamplerCreateInfo>());
    } else {
      stream.colorSampler = vkw.context.device.createSamplerUnique(
              vk::SamplerCreateInfo(
                {},
                vk::Filter::eLinear,
                vk::Filter::eLinear,
                vk::SamplerMipmapMode::eNearest,
                vk::SamplerAddressMode::eClampToEdge,
                vk::SamplerAddressMode::eClampToEdge,
                vk::SamplerAddressMode::eClampToEdge)
              .setBorderColor(vk::BorderColor::eFloatOpaqueBlack)
      );
    }
 
    auto makeSlot = [&](vk::Format format, glm::ivec2 res, bool yuv) {
      auto createInfo = vk::ImageCreateInfo{
        {},
        vk::ImageType::e2D,
        format,
        vk::Extent3D(res.x, res.y, 1),
        1, //Mip
        1, //Layers
        vk::SampleCountFlagBits::e1,
        vk::ImageTiling::eOptimal,
        yuv ? vk::ImageUsageFlagBits::eSampled : vk::ImageUsageFlagBits::eSampled| vk::ImageUsageFlagBits::eStorage ,
        vk::SharingMode::eExclusive,
        0,
        nullptr,
        vk::ImageLayout::eUndefined
      };
 
      auto imgExportInfo = vk::ExternalMemoryImageCreateInfo{memory_handle_type};
      auto infoChain = vk::StructureChain(createInfo, imgExportInfo);
 
      auto image = dev.createImageUnique(infoChain.get<vk::ImageCreateInfo>());
      auto memreqs = dev.getImageMemoryRequirements(*image);
      auto memIndex = findMemoryType(vkw.context.physicalDevice, memreqs.memoryTypeBits, vk::MemoryPropertyFlagBits::eDeviceLocal);
 
      auto allocInfo = vk::StructureChain(
            vk::MemoryAllocateInfo{memreqs.size, memIndex},
            vk::ExportMemoryAllocateInfo{memory_handle_type}
            );
 
      auto memory = dev.allocateMemoryUnique(allocInfo.get<vk::MemoryAllocateInfo>());
      dev.bindImageMemory(*image, *memory, 0);
 
      auto viewInfo = vk::ImageViewCreateInfo{
        {}, //Flags
        *image,
        vk::ImageViewType::e2D,
        format,
        {}, //Components
        { //Subresource range
          vk::ImageAspectFlagBits::eColor, // TODO change for depth
          0, //Base mip
          1, //Mip count
          0, //Base layer
          1, //Layer count
        }
      };
 
      vk::SamplerYcbcrConversionInfo yuvInfo(*stream.ycbcrConversion);
      if(yuv){
        viewInfo.setPNext(&yuvInfo);
      }
 
      auto imageView = dev.createImageViewUnique(viewInfo);
 
 
      //Do image memory transition
      transitionImageLayout(&vkw.context, *commandPool, *image, format, vk::ImageLayout::eUndefined, vk::ImageLayout::eShaderReadOnlyOptimal);
 
      return ImageSlot{
        .image = std::move(image),
        .memory = std::move(memory),
        .memoryRequirements = memreqs,
        .imageView = std::move(imageView),
      };
    };
 
    auto makeImageFromSlot = [](const ImageSlot& slot) {
      return StreamImage{
        .image = *slot.image,
        .view = *slot.imageView
      };
    };
 
    // Allocate slots
    for(int j = 0; j < param.slotCount; j++) {
      stream.colorSlots.push_back(makeSlot(sparam.colorFormat, sparam.colorResolution, yuv));
      stream.colorImages.push_back(makeImageFromSlot(stream.colorSlots.back()));
      stream.depthSlots.push_back(makeSlot(sparam.depthFormat, sparam.depthResolution, false));
      stream.depthImages.push_back(makeImageFromSlot(stream.depthSlots.back()));
    }
 
    stream.depthSampler = vkw.context.device.createSamplerUnique(
            vk::SamplerCreateInfo(
              {},
              vk::Filter::eNearest,
              vk::Filter::eNearest,
              vk::SamplerMipmapMode::eNearest,
              vk::SamplerAddressMode::eClampToEdge,
              vk::SamplerAddressMode::eClampToEdge,
              vk::SamplerAddressMode::eClampToEdge)
            .setBorderColor(vk::BorderColor::eFloatOpaqueBlack));
 
    sparam.colorSampler = *stream.colorSampler;
    sparam.depthSampler = *stream.depthSampler;
 
    // Get memory handles and export them
    auto exportMemoryHandles = [&](const Stream& stream, bool depth){
      std::vector<HvtStreamImageMemoryInfo> handles;
      const auto& imgs = depth ? stream.depthSlots : stream.colorSlots;
      for(const auto& img : imgs){
#ifdef WIN32
        auto handle = dev.getMemoryWin32HandleKHR(
              vk::MemoryGetWin32HandleInfoKHR(*img.memory, memory_handle_type));
#else
        auto handle = dev.getMemoryFdKHR(
                    vk::MemoryGetFdInfoKHR(*img.memory, memory_handle_type));
#endif
        handles.push_back(HvtStreamImageMemoryInfo{
                            .size = img.memoryRequirements.size,
                            .alignment = img.memoryRequirements.alignment,
                            .handle = handle
                          });
      }
 
      HvtStreamImagesExportInfo einfos{
        .streamIndex = (uint32_t)i,
        .depth = depth,
        .memoryType = (HvtImageMemoryType)memory_handle_type,
        .imagesCount = (uint32_t)handles.size(),
        .pImages = handles.data()
      };
      hvtCheck(hvt.exportStreamImages, context, &einfos);
    };
 
    exportMemoryHandles(stream, false);
    exportMemoryHandles(stream, true);
 
    streamStorage.push_back(std::move(stream));
    i++;
  }
}
 
void DynamicInputProvider::initSynchronisation() {
  for(auto& sync : syncInfos) {
    auto semInfoChain = vk::StructureChain(
          vk::SemaphoreCreateInfo(),
          vk::ExportSemaphoreCreateInfo(semaphore_handle_type)
    );
    auto semInfo = semInfoChain.get<vk::SemaphoreCreateInfo>();
    auto acquire = vkw.context.device.createSemaphoreUnique(semInfo);
    auto release = vkw.context.device.createSemaphoreUnique(semInfo);
 
    auto exportSem = [&](vk::Semaphore sem, HvtSemaphore& toGet){
      #ifdef WIN32
      auto handle = vkw.context.device.getSemaphoreWin32HandleKHR(vk::SemaphoreGetWin32HandleInfoKHR(sem, semaphore_handle_type));
      #else
      auto handle = vkw.context.device.getSemaphoreFdKHR(vk::SemaphoreGetFdInfoKHR(sem, semaphore_handle_type));
      #endif
      HvtSemaphoreExportInfo einfo{
        .semaphore = handle,
        .type = (HvtSemaphoreType)semaphore_handle_type
      };
      hvtCheck(hvt.exportSemaphore, context,
               &einfo, &toGet);
    };
 
    exportSem(*acquire, sync.acquireHvt);
    exportSem(*release, sync.releaseHvt);
 
    sync.acquireSemaphore = std::move(acquire);
    sync.releaseSempahore = std::move(release);
  }
}
 
std::vector<DynamicInputProvider::StreamParameters> DynamicInputProvider::enumerateStreamsParameters() const {
  return cachedStreamParameters;
}
 
std::vector<DynamicInputProvider::StreamImage> DynamicInputProvider::enumerateStreamImages(uint32_t streamIndex, bool depth) const {
  const auto& stream = streamStorage.at(streamIndex);
  if(depth) {
    return stream.depthImages;
  } else {
    return stream.colorImages;
  }
}
 
void DynamicInputProvider::acquireStreamsFrames(const Extrinsics& targetViewExtrinsics, std::span<StreamFrameInfo> outFrameInfos) {
  auto extrinsicsToHvt = [](Extrinsics ex){
    return HvtExtrinsics{
      .position = HvtPosition{
          .x = ex.position.x,
          .y = ex.position.y,
          .z = ex.position.z
      },
      .rotation = HvtRotation{
          .yaw = ex.rotation.x,
          .pitch = ex.rotation.y,
          .roll = ex.rotation.z
      }
    };
  };
 
  // Take next sync slot
  currentSyncSlot = (currentSyncSlot+1) % syncInfos.size();
  auto& currentSync = syncInfos.at(currentSyncSlot);
 
  // Get stream frame info from the module
  std::vector<HvtStreamFrameInfo> hvtFrameInfos(outFrameInfos.size());
  HvtAcquireStreamFramesInfo ainfos{
    .viewerExtrinsics = extrinsicsToHvt(targetViewExtrinsics),
    .frameInfoCount = (uint32_t)hvtFrameInfos.size(),
    .pStreamFrameInfos = hvtFrameInfos.data(),
    .signalSemaphore = currentSync.acquireHvt
  };
  hvtCheck(hvt.acquireStreamsFrames, context, &ainfos);
 
 
  //Fill the output frames info
  for(int i = 0; i < outFrameInfos.size(); i++) {
    const auto& hfi = hvtFrameInfos.at(i);
    auto& fi = outFrameInfos[i];
 
    auto ext = hfi.extrinsics;
    auto intr = hfi.intrinsics;
 
    if (cachedStreamParameters.at(i).projectionType == ProjectionType::PROJECTION_PERSPECTIVE) {
        fi = StreamFrameInfo{
            .imageIndex = hfi.imageIndex,
            .extrinsics = Extrinsics{
              .position = {ext.position.x,ext.position.y,ext.position.z},
              .rotation = {ext.rotation.yaw, ext.rotation.pitch, ext.rotation.roll}
            },
            .intrinsics = Intrinsics{ //TODO handle the equirectangular case
              PerspectiveIntrinsics{
                .focals = {hfi.intrinsics.perspective.focalX, hfi.intrinsics.perspective.focalY},
                .principle = {hfi.intrinsics.perspective.principlePointX, hfi.intrinsics.perspective.principlePointY}
              }
            }
        };
    }
    else if(cachedStreamParameters.at(i).projectionType == ProjectionType::PROJECTION_EQUIRECTANGULAR) {
        fi = StreamFrameInfo{
            .imageIndex = hfi.imageIndex,
            .extrinsics = Extrinsics{
              .position = {ext.position.x,ext.position.y,ext.position.z},
              .rotation = {ext.rotation.yaw, ext.rotation.pitch, ext.rotation.roll}
            },
            .intrinsics = Intrinsics{ 
              EquirectangularIntrinsics{
                .verticalRange = {hfi.intrinsics.equirectangular.verticalRange[0], hfi.intrinsics.equirectangular.verticalRange[1]},
                .horizontalRange = {hfi.intrinsics.equirectangular.horizontalRange[0], hfi.intrinsics.equirectangular.horizontalRange[1]}
              }
            }
        };
    }
    else {
        throw std::runtime_error("Invalid projection type !");
    }
  }
 
  //Wait on the image sempahore
  vk::PipelineStageFlags waitStage = vk::PipelineStageFlagBits::eTopOfPipe;
  vkw.context.graphicsQueue.submit(
        vk::SubmitInfo(*currentSync.acquireSemaphore, waitStage));
}
 
void DynamicInputProvider::releaseStreamsFrames() {
  auto& currentSync = syncInfos.at(currentSyncSlot);
 
  // Submit a signal that we are done with the image
  vkw.context.graphicsQueue.submit(
        vk::SubmitInfo({},{},{},*currentSync.releaseSempahore));
 
  hvtCheck(hvt.releaseStreamsFrames, context, currentSync.releaseHvt); //TODO pass proper semaphore
}
 
DynamicInputProvider::~DynamicInputProvider() {
  hvtCheck(hvt.stopStreaming, context);
  hvtCheck(hvt.destroyStreamingContext, context);
}