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));
      });
    }


    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> 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 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<const CExports::CVC3DSensorSettings *>(&settings), encoderStep,
            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 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] = {};  // NOLINT(cppcoreguidelines-avoid-c-arrays)
        std::copy(planeIncs.begin(), planeIncs.end(), std::begin(tmpPlaneIncs));
        return CVB_CALL_CAPI(CVC3DCreateSparsePointCloudFromPointer(
            const_cast<void **>(planeBasePtrs.data()),  // NOLINT(cppcoreguidelines-pro-type-const-cast)
            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
 
}  // namespace Cvb