decl_point_cloud.hpp

#pragma once
 
#include <unordered_map>
#include <limits>
 
#include "../_cexports/c_core_3d.h"
 
#include "decl_composite.hpp"
 
#include "../affine_matrix_3d.hpp"
#include "../cuboid.hpp"
#include "../data_type.hpp"
#include "../exception.hpp"
#include "../global.hpp"
#include "../image.hpp"
#include "../matrix_3d.hpp"
#include "../matrix_3d_h.hpp"
#include "../point_3d.hpp"
#include "../point_3d_h.hpp"
#include "../size_2d.hpp"
#include "../plane.hpp"
#include "../utilities/system_info.hpp"
#include "../components_pointers_3d.hpp"
 
#include "../plane_3d.hpp"
 
#include "../calibrator_3d.hpp"
#include "../core_3d.hpp"
 
namespace Cvb
{
 
  CVB_BEGIN_INLINE_NS
 
  namespace detail
  {
    // Backport std::is_invocable_r for C++14
    template <typename R, typename F, typename... Args>
    struct is_invocable_r
        : std::is_constructible<std::function<R(Args...)>,
                                std::reference_wrapper<typename std::remove_reference<F>::type>>
    {
    };
  }  // namespace detail
 
  template <>
  inline HandleGuard<PointCloud>::HandleGuard(void *handle) noexcept
      : HandleGuard<PointCloud>(handle, [](void *handle) { CVB_CALL_CAPI(ReleaseObject(handle)); })
  {
  }

  class PointCloud
  {
  private:
    template <class Point>
    static PointCloudFlags PointTypeToFlags()
    {
      if (std::is_same<Point, Cvb::Point3D<float>>::value)
        return PointCloudFlags::XYZ | PointCloudFlags::Float;
      else if (std::is_same<Point, Cvb::Point3D<double>>::value)
        return PointCloudFlags::XYZ | PointCloudFlags::Double;
      else if (std::is_same<Point, Cvb::Point3DC<float>>::value)
        return PointCloudFlags::XYZConfidence | PointCloudFlags::Float;
      else if (std::is_same<Point, Cvb::Point3DC<double>>::value)
        return PointCloudFlags::XYZConfidence | PointCloudFlags::Double;
      else if (std::is_same<Point, Cvb::Point3DH<float>>::value)
        return PointCloudFlags::XYZW | PointCloudFlags::Float;
      else if (std::is_same<Point, Cvb::Point3DH<double>>::value)
        return PointCloudFlags::XYZW | PointCloudFlags::Double;
      else
        throw std::invalid_argument("Point must be one of the Cvb::Point3D types.");
    }
 
  protected:
    struct PrivateTag
    {
    };
 
  public:
    using GuardType = HandleGuard<PointCloud>;


    static PointCloudPtr FromHandle(HandleGuard<PointCloud> &&guard);
 
    template <class T>
    static std::shared_ptr<T> FromHandle(HandleGuard<PointCloud> &&guard)
    {
      // required for generic object creation
      return T::FromHandle(std::move(guard));
    }


    template <class T>
    static std::shared_ptr<T> FromComposite(CompositePtr object);


    static PointCloudPtr FromMemory(void *buffer, size_t size, PointCloudFlags flags, Cvb::PointCloudFileFormat format);


    template <class Point>
    static PointCloudPtr FromMemory(void *buffer, size_t size, Cvb::PointCloudFileFormat format)
    {
      auto flags = PointTypeToFlags<Point>();
      return FromMemory(buffer, size, flags, format);
    }


    PointCloudLayout Layout() const
    {
      CExports::CVC3DPointCloudLayout layout = CExports::CVC3DPCL_Invalid;
      Internal::DoResCall([&]() { return CVB_CALL_CAPI(CVC3DPointCloudAnalyzeLayout(Handle(), layout)); });
      return static_cast<PointCloudLayout>(layout);
    }


    class DataType DataType() const
    {
      CExports::cvbdatatype_t dt = 0;
      Internal::DoResCall([&]() { return CVB_CALL_CAPI(CVC3DPointCloudGetDatatype(Handle(), dt)); });
      return Cvb::DataType::FromNativeDescriptor(static_cast<int>(dt));
    }


    std::size_t NumPoints() const
    {
      CExports::cvbint64_t numPoints = 0;
      Internal::DoResCall([&]() { return CVB_CALL_CAPI(CVC3DPointCloudGetNumPoints(Handle(), numPoints)); });
      return static_cast<std::size_t>(numPoints);
    }


    ComponentsPointers3D PointComponents() const
    {
      ComponentsPointers3D components;
      auto error = TryPointComponents(components);
      if (error)
        std::rethrow_exception(error);
      return components;
    }


    template <class T>
    std::size_t Points(Point3D<T> *&points) const
    {
      return PointsTyped(points);
    }


    template <class T>
    std::size_t PointsH(Point3DH<T> *&pointsH) const
    {
      return PointsHTyped(pointsH);
    }


    Cuboid CalculateBoundingBox(bool onlyConfidentPoints = true) const
    {
      if (onlyConfidentPoints)
      {
        for (const auto p : Planes())
        {
          if (p->Role() == PlaneRole::PixConfidence)
            return CalculateBoundingBox([](double confidence) {
              return std::fabs(confidence - 1.0) <= std::numeric_limits<double>::epsilon();
            });
        }
      }
 
      Cuboid boundingBox;
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(
            CVC3DPointCloudCalculateBoundingBox(Handle(), *reinterpret_cast<CExports::CVC3DCuboid *>(&boundingBox)));
      });
      return boundingBox;
    }


    template <typename FN,
              typename std::enable_if_t<detail::is_invocable_r<bool, FN, double, double, double>::value> * = nullptr>
    Cuboid CalculateBoundingBox(const FN &xyzPredicate) const
    {
      using FNType = std::conditional<std::is_function<FN>::value, FN, std::add_const<FN>::type>::type;
      thread_local FNType *fn = nullptr;
      auto predicate            = [](double x, double y, double z) -> bool { return (*fn)(x, y, z); };
 
      fn = &xyzPredicate;
 
      Cuboid boundingBox;
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(CVC3DPointCloudCalculateBoundingBoxFiltered(
            Handle(), predicate, *reinterpret_cast<CExports::CVC3DCuboid *>(&boundingBox)));
      });
 
      return boundingBox;
    }


    template <typename FN, typename std::enable_if_t<detail::is_invocable_r<bool, FN, double>::value> * = nullptr>
    Cuboid CalculateBoundingBox(const FN &confidencePredicate) const
    {
      using FNType = std::conditional<std::is_function<FN>::value, FN, std::add_const<FN>::type>::type;
      thread_local FNType *fn = nullptr;
      auto predicate            = [](double c) -> bool { return (*fn)(c); };
 
      fn = &confidencePredicate;
 
      Cuboid boundingBox;
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(CVC3DPointCloudCalculateBoundingBoxFilteredConfidence(
            Handle(), predicate, *reinterpret_cast<CExports::CVC3DCuboid *>(&boundingBox)));
      });
 
      return boundingBox;
    }


    Point3D<double> CalculateCenterOfGravity() const
    {
      Point3D<double> center;
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(
            CVC3DPointCloudCalculateCenterOfGravity(Handle(), *reinterpret_cast<CExports::CVC3DPointD *>(&center)));
      });
      return center;
    }


    Matrix3D CalculateCovarianceMatrix() const
    {
      Matrix3D matrix;
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(
            CVC3DPointCloudCalculateCovarianceMatrix(Handle(), *reinterpret_cast<CExports::CVC3DMatrix *>(&matrix)));
      });
      return matrix;
    }


    Plane3D FitPlane() const
    {
      return FitPlane(CalculateBoundingBox());
    }


    Plane3D FitPlane(const Cuboid &aoi) const
    {
      CExports::CVC3DPlane plane = {0};
      CExports::cvbres_t res     = CVB_CALL_CAPI(CVC3DPointCloudCalculatePlaneFromCuboid(
          Handle(), *reinterpret_cast<const CExports::CVC3DCuboid *>(&aoi), plane));
      if (res < 0)
        std::rethrow_exception(CvbException::FromCvbResult(res, "failed to calculate plane from cuboid."));
      return Plane3D(plane.Normal.X, plane.Normal.Y, plane.Normal.Z, plane.DistanceToOrigin);
    }


    template <class T>
    std::shared_ptr<T> Crop(const Cuboid &clipBox) const
    {
      return Internal::DoResCallShareOut<T>([&](void *&handle) {
        return CVB_CALL_CAPI(
            CVC3DCreateCroppedPointCloud(Handle(), *reinterpret_cast<const CExports::CVC3DCuboid *>(&clipBox), handle));
      });
    }


    template <class T>
    std::shared_ptr<T> FrustumCrop(const Cuboid &clipBox, Angle theta, Angle phi) const
    {
      return Internal::DoResCallShareOut<T>([&](void *&handle) {
        return CVB_CALL_CAPI(CVC3DCreateFrustumCroppedPointCloud(
            Handle(), *reinterpret_cast<const CExports::CVC3DCuboid *>(&clipBox), theta.Deg(), phi.Deg(), handle));
      });
    }


    template <class T>
    std::shared_ptr<T> PlaneCrop(const Plane3D &plane, const ValueRange<double> &range, CropRange cropRange) const
    {
      return Internal::DoResCallShareOut<T>([&](void *&handle) {
        return CVB_CALL_CAPI(
            CVC3DCreatePlaneCroppedPointCloud(Handle(), *reinterpret_cast<const CExports::CVC3DPlane *>(&plane),
                                              *reinterpret_cast<const CExports::CVC3DRange *>(&range),
                                              static_cast<CExports::CVC3DCropRange>(cropRange), handle));
      });
    }


    template <class T>
    std::shared_ptr<T> PlaneCrop(const Plane3D &plane, double threshold,
                                 CropDirection cropBelowAbove = CropDirection::Below) const
    {
      // get correct plane
      auto planeUsed = plane;
      if (std::fabs(plane.Normal().Z())
          < 1e-7)  // if plane is parallel to z axis above/below is not define -> no cropping is done
        return std::dynamic_pointer_cast<T>(this->Duplicate());
      else if (plane.Normal().Z() < 0)  // make sure, that z shows up to get right definition of above and below.
      {
        planeUsed.SetNormal(Point3D<double>(-plane.Normal().X(), -plane.Normal().Y(), -plane.Normal().Z()));
        planeUsed.SetDistanceToOrigin(-plane.DistanceToOrigin());
      }
 
      // get correct range
      auto range = ValueRange<double>(std::numeric_limits<double>::lowest(), threshold);
      if (cropBelowAbove == CropDirection::Above)
        range = ValueRange<double>(-threshold, std::numeric_limits<double>::max());
 
      return PlaneCrop<T>(planeUsed, range, CropRange::CropWithinRange);
    }


    PointCloudPtr Downsample(DownSampleMode mode, int value) const
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void *&handle) {
        return CVB_CALL_CAPI(CVC3DCreateDownsampledPointCloud(Handle(),
                                                              static_cast<CExports::CVC3DDownsampleMode>(mode),
                                                              static_cast<CExports::cvbint64_t>(value), handle));
      });
    }


    PointCloudPtr Duplicate() const
    {
      return Internal::DoResCallShareOut<PointCloud>(
          [&](void *&handle) { return CVB_CALL_CAPI(CVC3DCreateDuplicatePointCloud(Handle(), handle)); });
    }


    void Save(const String &fileName) const
    {
      Save(fileName, static_cast<PointCloudFlags>(0));
    }
 
    void Save(const String &fileName, PointCloudFlags flags) const
    {
      Internal::DoResCall([&]() {
        return CVB_CALL_CAPI(CVC3DWriteFileTyped(Handle(), static_cast<CExports::cvbval_t>(flags), fileName.c_str()));
      });
    }


    PointCloudPtr Convert(PointCloudFlags flags) const
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void *&handle) {
        return CVB_CALL_CAPI(CVC3DCreateConvertedPointCloud(Handle(), static_cast<CExports::cvbval_t>(flags), handle));
      });
    }


    PointCloudPtr Scale(Factors3D factors) const
    {
      return Internal::DoResCallShareOut<PointCloud>([&](void *&handle) {
        return CVB_CALL_CAPI(
            CVC3DCreateScaledPointCloud(Handle(), *reinterpret_cast<CExports::CVC3DFactors *>(&factors), handle));
      });
    }


    SparsePointCloudPtr Transform(const AffineMatrix3D &transformation) const;


    SparsePointCloudPtr Transform(const Matrix3DH &transformation) const;


    void Transform(const AffineMatrix3D &affineTransformation, SparsePointCloud &pointCloud);


    void Transform(const Cvb::Matrix3DH &transformation, SparsePointCloud &pointCloud);


    std::unique_ptr<Cvb::Image> RangeMap(ValueRange<double> xRange, ValueRange<double> yRange, Size2D<int> size,
                                         double background) const
    {
      return Internal::DoResCallObjectOut<Image>([&](void *&handle) {
        return CVB_CALL_CAPI(CVC3DCreateRangeMapFromPointCloud(
            Handle(), *reinterpret_cast<CExports::CVC3DRange *>(&xRange),
            *reinterpret_cast<CExports::CVC3DRange *>(&yRange), static_cast<CExports::cvbdim_t>(size.Width()),
            static_cast<CExports::cvbdim_t>(size.Height()), background, handle));
      });
    }


    int PlaneCount() const noexcept
    {
      CExports::cvbdim_t numPlanes = 0;
      CVB_CALL_CAPI(CVCPlaneEnumGetCount(Handle(), numPlanes));
      return static_cast<int>(numPlanes);
    }


    std::vector<PlanePtr> Planes() const
    {
      std::vector<PlanePtr> planes;
      std::generate_n(std::back_inserter(planes), PlaneCount(), [this, i = 0]() mutable { return Plane(i++); });
      return planes;
    }


    PlanePtr Plane(int index) const
    {
      if (index < 0 || index >= PlaneCount())
        throw std::length_error("index out of range");
 
      CExports::CVPLANE handle = nullptr;
      CVB_CALL_CAPI_CHECKED(CVCPlaneEnumGetAt(Handle(), static_cast<CExports::cvbdim_t>(index), handle));
      auto plane = Plane::FromHandle(HandleGuard<class Plane>(handle));
 
      auto iter = planes_.find(plane->Handle());
      if (iter != planes_.end())
      {
        if (auto _plane = iter->second.lock())
          return _plane;
      }
 
      planes_[plane->Handle()] = plane;
      return plane;
    }


    size_t MemorySize(PointCloudFlags flags, Cvb::PointCloudFileFormat format) const
    {
      std::size_t bufSize{};
      CExports::cvbres_t res =
          CVB_CALL_CAPI(CVC3DPointCloudToMemorySize(Handle(), static_cast<CExports::cvbval_t>(flags),
                                                    static_cast<CExports::CVC3DPointCloudFileFormat>(format), bufSize));
      if (res < 0)
        std::rethrow_exception(CvbException::FromCvbResult(res, "failed to get memory size."));
 
      return bufSize;
    }


    template <class Point>
    size_t MemorySize(Cvb::PointCloudFileFormat format) const
    {
      auto flags = PointTypeToFlags<Point>();
      return MemorySize(flags, format);
    }


    void ToMemory(PointCloudFlags flags, Cvb::PointCloudFileFormat format, size_t size, void *buffer) const
    {
      auto res = CVB_CALL_CAPI(CVC3DPointCloudToMemory(Handle(), static_cast<CExports::cvbval_t>(flags),
                                                       static_cast<CExports::CVC3DPointCloudFileFormat>(format), buffer,
                                                       size));
      if (res < 0)
        std::rethrow_exception(CvbException::FromCvbResult(res, "failed to save point cloud to memory."));
    }


    template <class Point>
    void ToMemory(Cvb::PointCloudFileFormat format, size_t size, std::uint8_t *buffer) const
    {
      auto flags = PointTypeToFlags<Point>();
      ToMemory(flags, format, size, buffer);
    }

    void *Handle() const noexcept
    {
      return handle_.Handle();
    }
 
    PointCloud(const PointCloud &other) noexcept            = delete;
    PointCloud &operator=(const PointCloud &other) noexcept = delete;
    PointCloud(PointCloud &&other) noexcept                 = delete;
    PointCloud &operator=(PointCloud &&other) noexcept      = delete;
 
    virtual ~PointCloud() = default;
 
  protected:
    explicit PointCloud(HandleGuard<PointCloud> &&guard) noexcept
        : handle_(std::move(guard))
    {
    }
 
    static bool IsDense(void *handle)
    {
      CExports::cvbdim_t width  = 0;
      CExports::cvbdim_t height = 0;
      auto res                  = CVB_CALL_CAPI(CVC3DPointCloudGetLatticeSize(handle, width, height));
      if (res == ErrorCodes::CVB_INVALIDDIMENSION)
        return false;
      else if (res < 0)
        Utilities::SystemInfo::ThrowLastError(res);
 
      return true;
    }
 
    std::exception_ptr TryPointComponents(ComponentsPointers3D &components) const noexcept
    {
      auto res = CVB_CALL_CAPI(CVC3DPointCloudGetPointComponentsPointers(
          Handle(), reinterpret_cast<void *&>(components.xBasePtr_), components.xInc_,
          reinterpret_cast<void *&>(components.yBasePtr_), components.yInc_,
          reinterpret_cast<void *&>(components.zBasePtr_), components.zInc_,
          reinterpret_cast<void *&>(components.wBasePtr_), components.wInc_, components.numPoints_));
      if (!CExports::CheckErrorCode(res))
        return Utilities::SystemInfo::GetLastError();
 
      CExports::cvbdatatype_t dataType = 0;
      CVB_CALL_CAPI(CVC3DPointCloudGetPointsConfidencePointer(
          Handle(), reinterpret_cast<void *&>(components.confidenceBasePtr_), components.confidenceInc_, dataType));
      return {};
    }
 
    template <class T>
    std::exception_ptr TryPointComponents(ComponentsPointers3D &components) const noexcept
    {
      auto error = TryPointComponents(components);
      if (error)
        return error;
 
      if (DataType() != DataType::FromNativeType<typename T::ValueType>())
        return std::make_exception_ptr(std::runtime_error("data type does not match for components pointers"));
 
#ifdef _MSC_VER
#  pragma warning(push, 1)
#  pragma warning(disable : 4127)  // conditional expression is constant
#endif
 
      if (std::is_same<T, Point3DH<typename T::ValueType>>::value && !components.BasePtrW())
        return std::make_exception_ptr(std::runtime_error("point cloud has no W components use Point3D"));
 
      if (std::is_same<T, Point3D<typename T::ValueType>>::value && components.BasePtrW())
        return std::make_exception_ptr(std::runtime_error("point cloud has W components use Point3DH"));
 
#ifdef _MSC_VER
#  pragma warning(pop)
#endif
 
      return {};
    }
 
  private:
    std::size_t PointsTyped(Cvb::Point3D<double> *&points) const
    {
      return Internal::DoResCallValueOut<std::size_t>([&](std::size_t &value) {
        return CVB_CALL_CAPI(
            CVC3DPointCloudGetPointDsPointer(Handle(), reinterpret_cast<CExports::CVC3DPointD *&>(points), value));
      });
    }
 
    std::size_t PointsTyped(Point3D<float> *&points) const
    {
      return Internal::DoResCallValueOut<std::size_t>([&](std::size_t &value) {
        return CVB_CALL_CAPI(
            CVC3DPointCloudGetPointFsPointer(Handle(), reinterpret_cast<CExports::CVC3DPointF *&>(points), value));
      });
    }
 
    std::size_t PointsHTyped(Cvb::Point3DH<double> *&pointsH)
    {
      return Internal::DoResCallValueOut<std::size_t>([&](std::size_t &value) {
        return CVB_CALL_CAPI(
            CVC3DPointCloudGetPointHDsPointer(Handle(), reinterpret_cast<CExports::CVC3DPointHD *&>(pointsH), value));
      });
    }
 
    std::size_t PointsHTyped(Cvb::Point3DH<float> *&pointsH)
    {
      return Internal::DoResCallValueOut<std::size_t>([&](std::size_t &value) {
        return CVB_CALL_CAPI(
            CVC3DPointCloudGetPointHFsPointer(Handle(), reinterpret_cast<CExports::CVC3DPointHF *&>(pointsH), value));
      });
    }
 
    mutable std::unordered_map<void *, std::weak_ptr<class Plane>> planes_;
 
    HandleGuard<PointCloud> handle_;
  };
 
  CVB_END_INLINE_NS
}  // namespace Cvb