point_cloud_factory.hpp

#pragma once

#include "_cexports/c_core_3d.h"

#include "global.hpp"
#include "string.hpp"
#include "core_3d.hpp"
#include "sparse_point_cloud.hpp"
#include "dense_point_cloud.hpp"
#include "calibrator_3d.hpp"
#include "sensor_settings.hpp"

namespace Cvb
{

CVB_BEGIN_INLINE_NS


class PointCloudFactory
{
  public:


    template <class T>
    static std::shared_ptr<T> Load(const String& fileName, PointCloudFlags flags)
    {
      static_assert(std::is_base_of<PointCloud, T>::value, "CVB: PointCloud must be a base of T");
      auto pointCloud = std::dynamic_pointer_cast<T>(Load(fileName, flags));
      if (!pointCloud)
        throw std::runtime_error("point cloud does not match type");
      return pointCloud;
    }


    static PointCloudPtr Load(const String& fileName, PointCloudFlags flags)
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void* & handle)
      {
        return CVB_CALL_CAPI(CVC3DLoadFileTyped(fileName.c_str(), static_cast<CExports::cvbval_t>(flags), handle));
      });
    }


    template <class T>
    static std::shared_ptr<T> Create(const ImagePlane& rangeMap, const Calibrator3D& calibrator, PointCloudFlags flags)
    {
      static_assert(std::is_base_of<PointCloud, T>::value, "CVB: PointCloud must be a base of T");
      auto pointCloud = Create(rangeMap, calibrator, flags);
      auto typedPointCloud = std::dynamic_pointer_cast<T>(pointCloud);
      if (!typedPointCloud)
        typedPointCloud = TypeConvert<T>::Convert(*pointCloud);
      return typedPointCloud;
    }


    static PointCloudPtr Create(const ImagePlane& rangeMap, const Calibrator3D& calibrator, PointCloudFlags flags)
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void* & handle)
      {
        return CVB_CALL_CAPI(CVC3DCreateCalibratedPointCloudFromRangeMap(rangeMap.Map()->Handle(), calibrator.Handle(), static_cast<CExports::cvbval_t>(flags), handle));
      });
    }


    template <class T>
    static std::shared_ptr<T> CreateWithSettings(const ImagePlane& rangeMap, const Calibrator3D& calibrator, PointCloudFlags flags, const SensorSettings& settings, double encoderStep)
    {
      static_assert(std::is_base_of<PointCloud, T>::value, "CVB: PointCloud must be a base of T");
      auto pointCloud = CreateWithSettings(rangeMap, calibrator, flags, settings, encoderStep);
      auto typedPointCloud = std::dynamic_pointer_cast<T>(pointCloud);
      if (!typedPointCloud)
        typedPointCloud = TypeConvert<T>::Convert(*pointCloud);
      return typedPointCloud;
    }


    static PointCloudPtr CreateWithSettings(const ImagePlane& rangeMap, const Calibrator3D& calibrator, PointCloudFlags flags, const SensorSettings& settings, double encoderStep)
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void*& handle)
        {
          return CVB_CALL_CAPI(CVC3DCreateCalibratedPointCloudFromRangeMapWithSettings(
            rangeMap.Map()->Handle(),
            calibrator.Handle(),
            *reinterpret_cast<CExports::CVC3DSensorSettings*>(const_cast<SensorSettings*>(&settings)),
            encoderStep,
            static_cast<CExports::cvbval_t>(flags), handle));
        });
    }


    static SparsePointCloudPtr CreateSparse(std::size_t numPoints, PointCloudFlags flags)
    {
      return std::dynamic_pointer_cast<SparsePointCloud>(Internal::DoResCallShareOut<PointCloud>([&](void * & handle)
      {
        return CVB_CALL_CAPI(CVC3DCreateSparsePointCloud(
          static_cast<CExports::cvbint64_t>(numPoints),
          static_cast<CExports::cvbval_t>(flags),
          handle));
      }));
    }


    static SparsePointCloudPtr CreateSparse(const std::vector<void*> & planeBasePtrs, int numPlanes, Cvb::DataType dataType, std::size_t numPoints, const std::vector<std::intptr_t> & planeIncs)
    {
      return std::dynamic_pointer_cast<SparsePointCloud>(Internal::DoResCallShareOut<PointCloud>([&](void * & handle)
      {
        std::intptr_t tmpPlaneIncs[4] = {};
        std::copy(planeIncs.begin(), planeIncs.end(), tmpPlaneIncs);
        return CVB_CALL_CAPI(CVC3DCreateSparsePointCloudFromPointer(
          const_cast<void**>(planeBasePtrs.data()),
          static_cast<CExports::cvbdim_t>(numPlanes),
          static_cast<CExports::cvbdatatype_t>(dataType.NativeDescriptor()),
          static_cast<std::size_t>(numPoints),
          tmpPlaneIncs,
          [](void *, void *) {},
          nullptr,
          handle));
      }));
    }


    static DensePointCloudPtr CreateDense(Size2D<int> size, PointCloudFlags flags)
    {
      return std::dynamic_pointer_cast<DensePointCloud>(Internal::DoResCallShareOut<PointCloud>([&](void * & handle)
      {
        return CVB_CALL_CAPI(CVC3DCreateDensePointCloud(size.Width(), size.Height(), static_cast<CExports::cvbval_t>(flags), handle));
      }));
    }



    static DensePointCloudPtr CreateDense(const Cvb::ImagePlane& rangeMap, Factors3D factors, PointCloudFlags flags)
    {
      return std::dynamic_pointer_cast<DensePointCloud>(Internal::DoResCallShareOut<PointCloud>([&](void* & handle)
      {
        return CVB_CALL_CAPI(CVC3DCreateDensePointCloudFromRangeMap(rangeMap.Map()->Handle(),
          *reinterpret_cast<CExports::CVC3DFactors*>(&factors), static_cast<CExports::cvbval_t>(flags),
          handle));
      }));

    }

    private:

      template <class T>
      struct TypeConvert{};
};


template <>
struct PointCloudFactory::TypeConvert<DensePointCloud>
{
  static DensePointCloudPtr Convert(const PointCloud& pointCloud)
  {
    const auto& sparse = dynamic_cast<const SparsePointCloud&>(pointCloud);
    return sparse.ToDensePointCloud();
  }
};

template <>
struct PointCloudFactory::TypeConvert<SparsePointCloud>
{
  static SparsePointCloudPtr Convert(const PointCloud& pointCloud)
  {
    const auto& dense = dynamic_cast<const DensePointCloud&>(pointCloud);
    return dense.ToSparsePointCloud();
  }
};


CVB_END_INLINE_NS

}