peak_icv_point_cloud.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <peak_icv/algorithms/calibration/peak_icv_calibration_result.hpp>
#include <peak_icv/exceptions/detail/peak_icv_exception_helpers.hpp>
#include <peak_icv/types/geometry/peak_icv_point_xyz.hpp>
#include <peak_icv/types/peak_icv_array_2d.hpp>
#include <peak_icv/utils/peak_icv_backend_accessor.hpp>
#include <peak_icv/utils/peak_icv_defines.hpp>
#include <peak_icv/utils/peak_icv_type_traits.hpp>
#include <peak_icv_c/types/peak_icv_point_cloud.h>
#include <algorithm>
#include <cstring>
#include <memory>
#include <vector>
 
namespace peak
{
namespace icv
{
 
namespace detail
{
 
template <typename PointCloudType, typename InnerPointType>
class PointCloud
    : private detail::IBackendAccessible<PointCloud<PointCloudType, InnerPointType>>
    , private detail::IBackendExchangeable<PointCloud<PointCloudType, InnerPointType>>
{
    static_assert(std::is_base_of<experimental::PointXYZ, InnerPointType>::value,
        "The given PointType is invalid. Please use a class which derives from Point3d");
 
public:
    using PointType = InnerPointType;
 
    ~PointCloud() override;
 
    PointCloud(const PointCloud& other);
 
    PointCloud(PointCloud&& other) noexcept;
 
    PointCloud& operator=(const PointCloud& other);
 
    PointCloud& operator=(PointCloud&& other) noexcept;
 
    bool operator==(const PointCloud& rhs) const;
 
    bool operator!=(const PointCloud& rhs) const;
 
    const std::vector<PointType>& GetPoints() const;
 
    PEAK_ICV_NO_DISCARD PointCloudType Transform(const experimental::TransformationMatrix3D& matrix) const;
 
 
    PEAK_ICV_NO_DISCARD PointCloudType TransformToWorkspace(const experimental::ExtrinsicParameters& extrinsicParameters) const;
 
    typename std::vector<PointType>::const_iterator cbegin() const;
 
    typename std::vector<PointType>::const_iterator cend() const;
 
protected:
    PointCloud();
    explicit PointCloud(peak_icv_point_cloud_handle handle);
 
    PEAK_ICV_NO_DISCARD detail::handle_of_t<PointCloud> GetHandle() const override;
 
    PEAK_ICV_NO_DISCARD detail::handle_of_t<PointCloud>* GetHandleAddress() override;
 
private:
    using PointTypeC = peak::common::detail::c_type_of_t<PointType>;
 
    peak_icv_point_cloud_handle m_pointCloudHandle{};
    mutable std::unique_ptr<std::vector<PointType>> m_points{};
 
    peak_icv_point_cloud_handle CreateEmptyPointCloud();
};
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>::~PointCloud()
{
    if (m_pointCloudHandle != nullptr)
    {
        PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_Destroy(m_pointCloudHandle);
    }
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>::PointCloud(peak_icv_point_cloud_handle handle)
    : m_pointCloudHandle{ handle }
{}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>::PointCloud(const PointCloud& other)
{
    if (m_pointCloudHandle != other.m_pointCloudHandle)
    {
        detail::ExecuteAndMapReturnCodes([&] {
            return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_IncreaseUseCount(other.m_pointCloudHandle);
        });
        m_pointCloudHandle = other.m_pointCloudHandle;
    }
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>::PointCloud(PointCloud&& other) noexcept
{
    if (m_pointCloudHandle != other.m_pointCloudHandle)
    {
        m_pointCloudHandle = other.m_pointCloudHandle;
        other.m_pointCloudHandle = nullptr;
    }
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>& PointCloud<PointCloudType, InnerPointType>::operator=(const PointCloud& other)
{
    if (m_pointCloudHandle != other.m_pointCloudHandle)
    {
        if (m_pointCloudHandle != nullptr)
        {
            detail::ExecuteAndMapReturnCodes([&] {
                return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_Destroy(m_pointCloudHandle);
            });
        }
        detail::ExecuteAndMapReturnCodes([&] {
            return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_IncreaseUseCount(other.m_pointCloudHandle);
        });
        m_pointCloudHandle = other.m_pointCloudHandle;
        m_points = std::make_unique<std::vector<InnerPointType>>(other.GetPoints());
    }
 
    return *this;
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>& PointCloud<PointCloudType, InnerPointType>::operator=(PointCloud&& other) noexcept
{
    if (m_pointCloudHandle != other.m_pointCloudHandle)
    {
        if (m_pointCloudHandle != nullptr)
        {
            detail::ExecuteAndMapReturnCodes([&] {
                return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_Destroy(m_pointCloudHandle);
            });
        }
 
        m_pointCloudHandle = other.m_pointCloudHandle;
        m_points = std::make_unique<std::vector<InnerPointType>>(other.GetPoints());
        other.m_pointCloudHandle = nullptr;
        other.m_points = nullptr;
    }
 
    return *this;
}
 
template <typename PointCloudType, typename InnerPointType>
bool PointCloud<PointCloudType, InnerPointType>::operator==(const PointCloud& rhs) const
{
    if (m_pointCloudHandle == rhs.m_pointCloudHandle)
    {
        return true;
    }
 
    auto pointLhs = this->GetPoints();
    auto pointRhs = rhs.GetPoints();
 
    if (pointLhs.size() != pointRhs.size())
    {
        return false;
    }
 
    return std::is_permutation(pointLhs.begin(), pointLhs.end(), pointRhs.begin());
}
 
template <typename PointCloudType, typename InnerPointType>
bool PointCloud<PointCloudType, InnerPointType>::operator!=(const PointCloud& rhs) const
{
    return !(*this == rhs);
}
 
template <typename PointCloudType, typename InnerPointType>
const std::vector<InnerPointType>& PointCloud<PointCloudType, InnerPointType>::GetPoints() const
{
    if (m_points)
    {
        return *m_points;
    }
 
    size_t numPoints = 0;
    detail::ExecuteAndMapReturnCodes([&] {
        return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_GetPoints_GetCount(m_pointCloudHandle, &numPoints);
    });
 
    if (numPoints == 0)
    {
        m_points = std::make_unique<std::vector<PointType>>(0);
        return *m_points;
    }
 
    std::vector<PointTypeC> points(numPoints);
    detail::ExecuteAndMapReturnCodes([&] {
        return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_GetPoints(m_pointCloudHandle, points.data(), sizeof(PointTypeC) * numPoints);
    });
 
    m_points = std::make_unique<std::vector<PointType>>(numPoints);
    std::transform(points.begin(), points.end(), m_points->begin(), [](const auto& point) {
        return peak::common::detail::BackendAccessor<PointType>::CreateInstance(point);
    });
 
    return *m_points;
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloudType PointCloud<PointCloudType, InnerPointType>::Transform(const experimental::TransformationMatrix3D& matrix) const
{
    peak_icv_transformation_matrix_3d cMatrix{};
    std::memcpy(&cMatrix, matrix.GetData(), sizeof(cMatrix));
 
    PointCloudType outputPointCloud{};
    auto outputHandle = peak::common::detail::BackendAccessor<PointCloudType>::BackendHandleAddress(outputPointCloud);
    detail::ExecuteAndMapReturnCodes([&] {
        return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_Transform(this->GetHandle(), cMatrix, outputHandle);
    });
    return outputPointCloud;
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloudType PointCloud<PointCloudType, InnerPointType>::TransformToWorkspace(
    const experimental::ExtrinsicParameters& extrinsicParameters) const
{
    const auto extrinsicParametersC = peak::common::detail::BackendAccessor<experimental::ExtrinsicParameters>::CreateCType(
        extrinsicParameters);
 
    PointCloudType outputPointCloud{};
    auto outputHandle = peak::common::detail::BackendAccessor<PointCloudType>::BackendHandleAddress(outputPointCloud);
    detail::ExecuteAndMapReturnCodes([&] {
        return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_TransformToWorkspace(this->GetHandle(), extrinsicParametersC, outputHandle);
    });
    return outputPointCloud;
}
 
template <typename PointCloudType, typename InnerPointType>
typename std::vector<InnerPointType>::const_iterator PointCloud<PointCloudType, InnerPointType>::cbegin() const
{
    return GetPoints().cbegin();
}
 
template <typename PointCloudType, typename InnerPointType>
typename std::vector<InnerPointType>::const_iterator PointCloud<PointCloudType, InnerPointType>::cend() const
{
    return GetPoints().cend();
}
 
template <typename PointCloudType, typename InnerPointType>
PointCloud<PointCloudType, InnerPointType>::PointCloud()
    : m_pointCloudHandle{ CreateEmptyPointCloud() }
{}
 
template <typename PointCloudType, typename InnerPointType>
detail::handle_of_t<PointCloud<PointCloudType, InnerPointType>> PointCloud<PointCloudType, InnerPointType>::GetHandle() const
{
    return m_pointCloudHandle;
}
 
template <typename PointCloudType, typename InnerPointType>
detail::handle_of_t<PointCloud<PointCloudType, InnerPointType>>* PointCloud<PointCloudType, InnerPointType>::GetHandleAddress()
{
    return &m_pointCloudHandle;
}
 
template <typename PointCloudType, typename InnerPointType>
peak_icv_point_cloud_handle PointCloud<PointCloudType, InnerPointType>::CreateEmptyPointCloud()
{
    peak_icv_point_cloud_handle handle{};
    detail::ExecuteAndMapReturnCodes([&] {
        return PEAK_ICV_C_ABI_PREFIX peak_icv_PointCloud_Create(&handle);
    });
    return handle;
}
 
} // namespace detail
 
 
} /* namespace icv */
} /* namespace peak */