pointcloud_transformation.h

Go to the documentation of this file.

 
#pragma once
 
#include "pointcloud_data.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <stdexcept>
#include <variant>

namespace adtf
{
namespace devicetb
{
namespace sdk
{
namespace pointcloud
{
 
namespace navigation
{
    
namespace detail
{
 
template<typename Output, typename Input>
struct Clamper
{
    static Output clamp(const Input& value) noexcept
    {
        static_assert(std::is_arithmetic_v<Output>, "Output must be arithmetic");
        static_assert(std::is_arithmetic_v<Input>, "Input must be arithmetic");
 
        if constexpr (std::is_same_v<Input, Output>)
        {
            return value;
        }
        else if constexpr (std::is_same_v<Input, bool> || std::is_same_v<Output, bool>)
        {
            return static_cast<Output>(value);
        }
        else
        {
            constexpr const auto max_output = std::numeric_limits<Output>::max();
            if constexpr (std::is_unsigned_v<Input> && std::is_unsigned_v<Output>)
            {
                return value > max_output ? max_output : static_cast<Output>(value);
            }
            else if constexpr (std::is_unsigned_v<Input> && std::is_signed_v<Output>)
            {
                return value >= max_output ? max_output : static_cast<Output>(value);
            }
            else if constexpr (std::is_signed_v<Input> && std::is_unsigned_v<Output>)
            {
                if (value < 0)
                {
                    return 0;
                }
                else
                {
                    return static_cast<uint64_t>(value) > max_output ? max_output : static_cast<Output>(value);
                }
            }
            else
            {
                if (value < 0)
                {
                    constexpr const auto min_output = std::numeric_limits<Output>::lowest();
                    return value <= min_output ? min_output : static_cast<Output>(value);
                }
                else
                {
                    if constexpr (max_output > std::numeric_limits<Input>::max())
                    {
                        return static_cast<Output>(value);
                    }
                    else
                    {
                        return value > max_output ? max_output : static_cast<Output>(value);
                    }
                }
            }
        }
    }
};
 
template <typename Output>
struct Clamper<Output, float16_t>
{
    static Output clamp(const float16_t& value) noexcept
    {
        return Clamper<Output, float>::clamp(static_cast<float>(value));
    }
};
 
template <typename Input>
struct Clamper<float16_t, Input>
{
    static float16_t clamp(const Input& value) noexcept
    {
        return float16_t(static_cast<double>(value));
    }
};
 
template <>
struct Clamper<float16_t, float16_t>
{
    static float16_t clamp(const float16_t& value) noexcept
    {
        return value;
    }
};
 
template<typename Output, typename Input>
Output clamp(const Input& value) noexcept
{
    return Clamper<Output, Input>::clamp(value);
}
 
template <typename SourceType, typename DestinationType>
void copy(const tAttribute::tInMemory& sSource,
          const tAttribute::tInMemory& sDestination,
          const void* const __restrict pSourceBuffer,
          void* const __restrict pDestinationBuffer,
          const size_t nCount)
{
    auto pReadPosition = static_cast<const std::byte*>(pSourceBuffer) + sSource.offset;
    auto pWritePosition = static_cast<std::byte*>(pDestinationBuffer) + sDestination.offset;
 
    if constexpr (std::is_same_v<SourceType, DestinationType>)
    {
        if (sSource.stride == sizeof(SourceType) &&
            sSource.stride == sDestination.stride &&
            sSource.endianess == sDestination.endianess)
        {
            std::memcpy(pWritePosition, pReadPosition, sizeof(SourceType) * nCount);
            return;
        }
    }
 
    if (sSource.endianess == get_byteorder())
    {
        if (sSource.endianess == sDestination.endianess)
        {
            for (size_t nIndex = 0; nIndex < nCount; ++nIndex)
            {
                *(reinterpret_cast<DestinationType*>(pWritePosition)) = clamp<DestinationType>(access<SourceType, false>(pReadPosition));
                pReadPosition += sSource.stride;
                pWritePosition += sDestination.stride;
            }
        }
        else
        {
            for (size_t nIndex = 0; nIndex < nCount; ++nIndex)
            {
                *(reinterpret_cast<DestinationType*>(pWritePosition)) = byte_swap(clamp<DestinationType>(access<SourceType, false>(pReadPosition)));
                pReadPosition += sSource.stride;
                pWritePosition += sDestination.stride;
            }
        }
    }
    else
    {
        if (sSource.endianess == sDestination.endianess)
        {
            if constexpr (std::is_same_v<SourceType, DestinationType>)
            {
                for (size_t nIndex = 0; nIndex < nCount; ++nIndex)
                {
                    *(reinterpret_cast<DestinationType*>(pWritePosition)) = *(reinterpret_cast<const SourceType*>(pReadPosition));
                    pReadPosition += sSource.stride;
                    pWritePosition += sDestination.stride;
                }
            }
            else
            {
                for (size_t nIndex = 0; nIndex < nCount; ++nIndex)
                {
                    *(reinterpret_cast<DestinationType*>(pWritePosition)) = byte_swap(clamp<DestinationType>(access<SourceType, true>(pReadPosition)));
                    pReadPosition += sSource.stride;
                    pWritePosition += sDestination.stride;
                }
            }
        }
        else
        {
            for (size_t nIndex = 0; nIndex < nCount; ++nIndex)
            {
                *(reinterpret_cast<DestinationType*>(pWritePosition)) = clamp<DestinationType>(access<SourceType, true>(pReadPosition));
                pReadPosition += sSource.stride;
                pWritePosition += sDestination.stride;
            }
        }
    }
}
 
template <typename SourceType>
void copy(const tAttribute::tInMemory& sSource,
          const tAttribute& sDestinationAttribute,
          const void* const __restrict pSourceBuffer,
          void* const __restrict pDestinationBuffer,
          const size_t nCount)
{
    switch (sDestinationAttribute.type)
    {
        case tAttribute::tDataType::dt_boolean: copy<SourceType, bool>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int8: copy<SourceType, int8_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint8: copy<SourceType, uint8_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int16: copy<SourceType, int16_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint16: copy<SourceType, uint16_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int32: copy<SourceType, int32_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint32: copy<SourceType, uint32_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int64: copy<SourceType, int64_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint64: copy<SourceType, uint64_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float16: copy<SourceType, float16_t>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float32: copy<SourceType, float>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float64: copy<SourceType, double>(sSource, sDestinationAttribute.sInMemory, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_computed_from_index: throw std::runtime_error("unexpected"); break;
    }
}
 
inline void copy(const tAttribute& sSourceAttribute,
                 const tAttribute& sDestinationAttribute,
                 const void* const __restrict pSourceBuffer,
                 void* const __restrict pDestinationBuffer,
                 const size_t nCount)
{
    switch (sSourceAttribute.type)
    {
        case tAttribute::tDataType::dt_boolean: copy<bool>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int8: copy<int8_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint8: copy<uint8_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int16: copy<int16_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint16: copy<uint16_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int32: copy<int32_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint32: copy<uint32_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_int64: copy<int64_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_uint64: copy<uint64_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float16: copy<float16_t>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float32: copy<float>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_float64: copy<double>(sSourceAttribute.sInMemory, sDestinationAttribute, pSourceBuffer, pDestinationBuffer, nCount); break;
        case tAttribute::tDataType::dt_computed_from_index: throw std::runtime_error("unexpected"); break;
    }
}
 
inline void copy_attributes(const tAttributes& oSourceAttributes,
                            const tAttributes& oDestAttributes,
                            const void* const __restrict pSourceData,
                            void* const __restrict pDestData,
                            const size_t nCount,
                            bool bIgnoreMissing = true)
{
    for (const auto& oTuple : oDestAttributes)
    {
        const auto& oDestAttr = std::get<tAttribute>(oTuple);
        const auto& strDestAttrName = std::get<std::string>(oTuple);
 
        if (oDestAttr.type == tAttribute::tDataType::dt_computed_from_index)
        {
            THROW_ERROR_DESC(ERR_INVALID_ARG, "The destination attribute %s is of type computed_from_index, which cannot be written to.",
                             strDestAttrName.c_str());
        }
 
        if (!oSourceAttributes.exists(strDestAttrName))
        {
            if (bIgnoreMissing)
            {
                continue;
            }
 
            THROW_ERROR_DESC(ERR_NOT_FOUND, "The source does not contain the %s attribute.", strDestAttrName.c_str());
        }
 
        const auto& oSourceAttr = oSourceAttributes.at(strDestAttrName);
 
        copy(oSourceAttr, oDestAttr, pSourceData, pDestData, nCount);
    }
}
 
template <bool exclude = true, typename Elements>
tAttributes filter(const tAttributes& oSourceAttributes, const Elements& elements)
{
    tAttributes oResult;
 
    if constexpr(exclude)
    {
        for (const auto& oAttribute : oSourceAttributes)
        {
            if (std::find(elements.begin(), elements.end(), std::get<0>(oAttribute)) ==
                elements.end())
            {
                oResult.add(std::get<0>(oAttribute), std::get<1>(oAttribute));
            }
        }
    }
    else
    {
        for (const auto& oAttribute : oSourceAttributes)
        {
            if (std::find(elements.begin(), elements.end(), std::get<0>(oAttribute)) !=
                elements.end())
            {
                oResult.add(std::get<0>(oAttribute), std::get<1>(oAttribute));
            }
        }
    }
    return oResult;
}
 
}
 
class cPointCloudTransformation
{
public:
    enum struct tTransformation
    {
        none = 0,
        copy_all = 1,
        copy_existing = 2,
        cartesian_to_polar = 3,
        polar_to_cartesian = 4,
    };
 
    static tTransformation GetSupportedTransformation(const tAttributes& oSourceAttributes,
                                                      const tAttributes& oDestAttributes)
    {
        const auto bAllDestinationAttributesInSource = [&]()
        {
            for (const auto& strDestinationAttribute: oDestAttributes)
            {
                if (!oSourceAttributes.exists(std::get<0>(strDestinationAttribute)))
                {
                    return false;
                }
            }
            return true;
        }();
 
        if (bAllDestinationAttributesInSource)
        {
            return tTransformation::copy_all;
        }
        else if (CanTransform<cTransformCartesianToPolar>(oSourceAttributes, oDestAttributes))
        {
            return tTransformation::cartesian_to_polar;
        }
        else if (CanTransform<cTransformPolarToCartesian>(oSourceAttributes, oDestAttributes))
        {
            return tTransformation::polar_to_cartesian;
        }
        else
        {
            return tTransformation::none;
        }
    }
 
    static void Transform(tTransformation eTransformType,
                          const tAttributes& oSourceAttributes,
                          const tAttributes& oDestAttributes,
                          const size_t nCount,
                          const detail::tConstBuffer& oSourceBuffer,
                          const detail::tBuffer& oDestinationBuffer)
    {
        switch (eTransformType)
        {
            case tTransformation::cartesian_to_polar:
            {
                cTransformCartesianToPolar::transform(oSourceAttributes, oDestAttributes, nCount, oSourceBuffer, oDestinationBuffer);
                break;
            }
            case tTransformation::polar_to_cartesian:
            {
                cTransformPolarToCartesian::transform(oSourceAttributes, oDestAttributes, nCount, oSourceBuffer, oDestinationBuffer);
                break;
            }
            case tTransformation::copy_all:
            {
                detail::copy_attributes(oSourceAttributes, oDestAttributes, oSourceBuffer.pData, oDestinationBuffer.pData, nCount);
                break;
            }
            case tTransformation::copy_existing:
            {
                detail::copy_attributes(oSourceAttributes, oDestAttributes, oSourceBuffer.pData, oDestinationBuffer.pData, nCount, true);
                break;
            }
            case tTransformation::none:
            {
                break;
            }
        }
    }
 
    static void Transform(const cPointCloudTypeHelper::tPointCloudAccess& oSource,
                          cPointCloudTypeHelper::tPointCloudAccess& oDestination,
                          bool bIgnoreMissingAdditionalAttributes = false)
    {
        if (*oSource.GetCount() != *oDestination.GetCount())
        {
            THROW_ERROR_DESC(ERR_INVALID_ARG,
                             "The layouts have no matching amount of points within their pointclouds (%zu vs. %zu).",
                             *oSource.GetCount(), *oDestination.GetCount());
        }
 
        const auto& oSourceAttributes = oSource.GetAttributes();
        const auto& oDestAttributes = oDestination.GetAttributes();
 
        const auto nSourceBufferSize = oSource.GetBufferSize();
        const auto nDestBufferSize = oDestination.GetBufferSize();
 
        // Best case scenario, we are fully layout compatible.
        if (oSourceAttributes == oDestAttributes)
        {
            if (nSourceBufferSize != nDestBufferSize)
            {
                THROW_ERROR_DESC(ERR_MEMORY, "Can only copy buffers with same sizes (%zu vs. %zu).",
                                 nSourceBufferSize, nDestBufferSize);
            }
 
            std::memcpy(oDestination.GetMutableData(), oSource.GetData(), nSourceBufferSize);
        }
        else
        {
            using namespace std::literals;
            static const std::array<std::string, 6> oMandatoryNames{"x"s, "y"s, "z"s, "distance"s, "yaw"s, "pitch"s};
 
            const auto oMandatoryAttributes = detail::filter<false>(oDestAttributes, oMandatoryNames);
            const auto oAdditionalAttributes = detail::filter<true>(oDestAttributes, oMandatoryNames);
 
            const auto eMandatoryTransformType = cPointCloudTransformation::GetSupportedTransformation(oSourceAttributes,
                                                                                                  oMandatoryAttributes);
            if (eMandatoryTransformType == cPointCloudTransformation::tTransformation::none)
            {
                THROW_ERROR_DESC(ERR_NOT_SUPPORTED, "No transformation for mandatory attributes found.");
            }
 
            const auto nCount = *oSource.GetCount();
            Transform(eMandatoryTransformType,
                      oSourceAttributes,
                      oMandatoryAttributes,
                      nCount,
                      detail::tConstBuffer(oSource.GetData(), nSourceBufferSize),
                      detail::tBuffer(oDestination.GetMutableData(), nDestBufferSize));
 
            if (!oAdditionalAttributes.empty())
            {
                auto eAdditionlTransformType = cPointCloudTransformation::GetSupportedTransformation(oSourceAttributes,
                                                                                                oAdditionalAttributes);
                if (eAdditionlTransformType == cPointCloudTransformation::tTransformation::none)
                {
                    if (!bIgnoreMissingAdditionalAttributes)
                    {
                        THROW_ERROR_DESC(ERR_NOT_SUPPORTED, "No transformation for additonal attributes found.");
                    }
 
                    eAdditionlTransformType = cPointCloudTransformation::tTransformation::copy_existing;
                }
 
                Transform(eAdditionlTransformType,
                          oSourceAttributes,
                          oAdditionalAttributes,
                          nCount,
                          detail::tConstBuffer(oSource.GetData(), nSourceBufferSize),
                          detail::tBuffer(oDestination.GetMutableData(), nDestBufferSize));
            }
        }
    }
 
private:
 
    template <typename Transform>
    static bool CanTransform(const tAttributes& oSourceAttributes,
                      const tAttributes& oDestAttributes)
    {
        for (const auto& strSourceElement: Transform::required_source_elements)
        {
            if (!oSourceAttributes.exists(strSourceElement))
            {
                return false;
            }
        }
 
        for (const auto& strDestinationElement: Transform::required_destination_elements)
        {
            if (!oDestAttributes.exists(strDestinationElement) ||
                 oDestAttributes.at(strDestinationElement).type == tAttribute::tDataType::dt_computed_from_index)
            {
                return false;
            }
        }
 
        return true;
    }
 
    struct cTransformCartesianToPolar
    {
        static constexpr std::array<const char*, 3> required_source_elements = {"x", "y", "z"};
        static constexpr std::array<const char*, 3> required_destination_elements = {"distance", "yaw", "pitch"};

        static void transform(const tAttributes& oSourceAttributes,
                              const tAttributes& oDestAttributes,
                              const size_t& nCount,
                              const detail::tConstBuffer& oSourceBuffer,
                              const detail::tBuffer& oDestinationBuffer)
        {
            using namespace std::literals;
            const auto& oX = oSourceAttributes.at("x"s);
            const auto& oY = oSourceAttributes.at("y"s);
            const auto& oZ = oSourceAttributes.at("z"s);
 
            const auto& oDistance = oDestAttributes.at("distance"s);
            const auto& oYaw = oDestAttributes.at("yaw"s);
            const auto& oPitch = oDestAttributes.at("pitch"s);
 
            for (size_t nIdx = 0; nIdx < nCount; nIdx++)
            {
                // Read values
                const auto x = detail::to_type<double>(get_attribute_value(oX, oSourceBuffer, nIdx));
                const auto y = detail::to_type<double>(get_attribute_value(oY, oSourceBuffer, nIdx));
                const auto z = detail::to_type<double>(get_attribute_value(oZ, oSourceBuffer, nIdx));
 
                // Actually calculate
                const auto distance = std::sqrt(std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2));
                const auto yaw = std::atan2(y, x);
                double pitch;
                distance == 0.0 ? pitch = 0.0 : pitch = std::asin(z / distance);
 
                // Write values
                set_attribute_value(oDistance, oDestinationBuffer, nIdx, distance);
                set_attribute_value(oYaw, oDestinationBuffer,  nIdx, yaw);
                set_attribute_value(oPitch, oDestinationBuffer,  nIdx, pitch);
            }
        }
    };
 
    struct cTransformPolarToCartesian
    {
        static constexpr std::array<const char*, 3> required_source_elements = {"distance", "yaw", "pitch"};
        static constexpr std::array<const char*, 3> required_destination_elements = {"x", "y", "z"};

        static void transform(const tAttributes& oSourceAttributes,
                              const tAttributes& oDestAttributes,
                              const size_t& nCount,
                              const detail::tConstBuffer& oSourceBuffer,
                              const detail::tBuffer& oDestinationBuffer)
        {
            using namespace std::literals;
 
            const auto& oDistance = oSourceAttributes.at("distance"s);
            const auto& oYaw = oSourceAttributes.at("yaw"s);
            const auto& oPitch = oSourceAttributes.at("pitch"s);
 
            const auto& oX = oDestAttributes.at("x"s);
            const auto& oY = oDestAttributes.at("y"s);
            const auto& oZ = oDestAttributes.at("z"s);
 
            for (size_t nIdx = 0; nIdx < nCount; ++nIdx)
            {
                // Read values
                const auto distance = detail::to_type<double>(get_attribute_value(oDistance, oSourceBuffer, nIdx));
                const auto yaw = detail::to_type<double>(get_attribute_value(oYaw, oSourceBuffer, nIdx));
                const auto pitch = detail::to_type<double>(get_attribute_value(oPitch, oSourceBuffer, nIdx));
 
                // Actually calculate
                const auto x = distance * std::cos(pitch) * std::cos(yaw);
                const auto y = distance * std::cos(pitch) * std::sin(yaw);
                const auto z = distance * std::sin(pitch);
 
                // Write values
                set_attribute_value(oX, oDestinationBuffer, nIdx, x);
                set_attribute_value(oY, oDestinationBuffer, nIdx, y);
                set_attribute_value(oZ, oDestinationBuffer, nIdx, z);
            }
        }
    };
};
 
} // namespace navigation
 
using cPointCloudTransformation = navigation::cPointCloudTransformation;
} // namespace pointcloud
} // namespace sdk
} // namespace devicetb
} // namespace adtf