VisitPointLikeContainer.h

Go to the documentation of this file.

#pragma once
 
#include <memory>
 
#include <boost/preprocessor/seq/for_each.hpp>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#pragma GCC diagnostic pop
 
#include <ArmarXCore/util/CPPUtility/TemplateMetaProgrammingBoost.h>
 
//point traits
namespace armarx::meta
{
 
#define _detail_unpack(r, data, elem) , elem
    template <class T>
    static constexpr bool is_pcl_point_type_v =
        is_any_of_v<T /*no ,*/ BOOST_PP_SEQ_FOR_EACH(_detail_unpack, , PCL_XYZ_POINT_TYPES)>;
#undef _detail_unpack
 
    template <class T, class = void>
    struct is_eigen_vec3_type : std::false_type
    {
    };
 
    template <class T>
    struct is_eigen_vec3_type<Eigen::Matrix<T, 3, 1>, void> : std::true_type
    {
    };
 
    template <class T>
    struct is_eigen_vec3_type<Eigen::Matrix<T, 1, 3>, void> : std::true_type
    {
    };
 
    template <class T>
    static constexpr bool is_eigen_vec3_type_v = is_eigen_vec3_type<T>::value;
 
    template <class T, class = void>
    struct point_type_traits : std::false_type
    {
    };
 
    template <class T>
    struct point_type_traits<T, std::enable_if_t<is_pcl_point_type_v<T>>> : std::true_type
    {
        static const auto&
        x(const T& v)
        {
            return v.x;
        }
 
        static const auto&
        y(const T& v)
        {
            return v.y;
        }
 
        static const auto&
        z(const T& v)
        {
            return v.z;
        }
    };
 
    template <class T>
    struct point_type_traits<T, std::enable_if_t<is_eigen_vec3_type_v<T>>> : std::true_type
    {
        static const auto&
        x(const T& v)
        {
            return v.x();
        }
 
        static const auto&
        y(const T& v)
        {
            return v.y();
        }
 
        static const auto&
        z(const T& v)
        {
            return v.z();
        }
    };
 
    template <>
    struct point_type_traits<CGAL::Epick::Point_3, void> : std::true_type
    {
        using T = CGAL::Epick::Point_3;
 
        static const auto&
        x(const T& v)
        {
            return v.x();
        }
 
        static const auto&
        y(const T& v)
        {
            return v.y();
        }
 
        static const auto&
        z(const T& v)
        {
            return v.z();
        }
    };
 
    template <class T>
    static constexpr bool is_point_type_v = point_type_traits<T>::value;
 
    static_assert(is_point_type_v<pcl::PointXYZ>);
    static_assert(is_point_type_v<Eigen::Vector3f>);
    static_assert(is_point_type_v<Eigen::Vector3i>);
    static_assert(is_point_type_v<Eigen::RowVector3cd>);
    static_assert(is_point_type_v<CGAL::Epick::Point_3>);
} // namespace armarx::meta
 
//container traits
#include <deque>
#include <forward_list>
#include <list>
#include <set>
#include <unordered_set>
#include <vector>
 
namespace armarx::meta
{
#define _detail_containerlist                                                                      \
    std::vector, std::deque, std::list, std::forward_list, std::set, std::unordered_set
    static_assert(!TypeTemplateTraits::IsInstanceOfAnyV<int, _detail_containerlist>);
    static_assert(TypeTemplateTraits::IsInstanceOfAnyV<std::vector<int>, _detail_containerlist>);
 
    template <class T, class = void>
    struct container_traits : std::false_type
    {
    };
 
    template <class T>
    struct container_traits<
        T,
        std::enable_if_t<TypeTemplateTraits::IsInstanceOfAnyV<T, _detail_containerlist>>> :
        std::true_type
    {
        using element_t = typename T::value_type;
 
        static auto
        begin(const T& t)
        {
            return t.begin();
        }
 
        static auto
        begin(T& t)
        {
            return t.begin();
        }
 
        static auto
        end(const T& t)
        {
            return t.end();
        }
 
        static auto
        end(T& t)
        {
            return t.end();
        }
 
        static const auto
        deref(auto& it)
        {
            return *it;
        }
 
        static auto
        size(const T& t)
        {
            return t.size();
        }
    };
 
#undef _detail_containerlist
 
    template <class PType>
    struct container_traits<pcl::PointCloud<PType>, void> : std::true_type
    {
        using T = pcl::PointCloud<PType>;
        using element_t = PType;
 
        static auto
        begin(const T& t)
        {
            return t.points.begin();
        }
 
        static auto
        begin(T& t)
        {
            return t.points.begin();
        }
 
        static auto
        end(const T& t)
        {
            return t.points.end();
        }
 
        static auto
        end(T& t)
        {
            return t.points.end();
        }
 
        static const auto
        deref(auto& it)
        {
            return *it;
        }
 
        static auto
        size(const T& t)
        {
            return t.points.size();
        }
    };
 
    template <class T>
    static constexpr bool is_container_type_v = container_traits<T>::value;
} // namespace armarx::meta
 
namespace armarx::detail::VisitPointLike
{
    static_assert(meta::is_container_type_v<std::vector<Eigen::Vector3f>>);
    static_assert(meta::is_container_type_v<std::vector<Eigen::Vector3d>>);
    static_assert(meta::is_container_type_v<std::vector<Eigen::RowVector3cd>>);
    static_assert(meta::is_container_type_v<pcl::PointCloud<pcl::PointXYZ>>);
 
    template <class T, class = void>
    struct VisitPointLike : std::false_type
    {
        //template<class F, class F2>
        //static void visit(const T& c, F& sizeinfo, F2& perElem);
        //{
        //    sizeinfo(c.size());
        //  perElem(p.x(), p.y(), p.z());
        //}
    };
 
    template <class T>
    struct VisitPointLike<
        T,
        std::enable_if_t<meta::is_container_type_v<T> &&
                         meta::is_point_type_v<typename meta::container_traits<T>::element_t>>> :
        std::true_type
    {
        template <class F, class F2>
        static void
        visit(const T& c, F& sizeinfo, F2& perElem)
        {
            using ctr = meta::container_traits<T>;
            using tr = meta::point_type_traits<typename ctr::element_t>;
            sizeinfo(ctr::size(c));
            auto f = ctr::begin(c);
            auto l = ctr::end(c);
            for (; f != l; ++f)
            {
                auto&& p = ctr::deref(f);
                perElem(tr::x(p), tr::y(p), tr::z(p));
            }
        }
    };
 
    template <class T>
    struct VisitPointLike<
        T,
        std::enable_if_t<VisitPointLike<meta::pointer_type_pointee_t<T>>::value>> : std::true_type
    {
        template <class F, class F2>
        static void
        visit(const T& c, F& sizeinfo, F2& perElem)
        {
            ARMARX_CHECK_NOT_NULL(c);
            VisitPointLike<meta::pointer_type_pointee_t<T>>::visit(*c, sizeinfo, perElem);
        }
    };
 
    template <class K>
    struct VisitPointLike<CGAL::Polyhedron_3<K>, void> : std::true_type
    {
        template <class F, class F2>
        static void
        visit(const CGAL::Polyhedron_3<K>& c, F& sizeinfo, F2& perElem)
        {
            sizeinfo(c.size_of_vertices());
            for (auto v = c.vertices_begin(); v != c.vertices_end(); ++v)
            {
                perElem(v->point().x(), v->point().y(), v->point().z());
            }
        }
    };
 
    template <class T>
    struct VisitPointLike<const T> : VisitPointLike<T>
    {
    };
 
    template <class T>
    struct VisitPointLike<T&> : VisitPointLike<T>
    {
    };
 
    static_assert(VisitPointLike<std::vector<Eigen::Matrix<float, 3, 1>,
                                             std::allocator<Eigen::Matrix<float, 3, 1>>>>::value);
    static_assert(VisitPointLike<std::vector<Eigen::Vector3f>>::value);
    static_assert(VisitPointLike<std::vector<Eigen::Vector3d>>::value);
    static_assert(VisitPointLike<std::vector<Eigen::RowVector3cd>>::value);
    static_assert(VisitPointLike<pcl::PointCloud<pcl::PointXYZ>>::value);
    static_assert(VisitPointLike<const std::vector<Eigen::Vector3f>&>::value);
} // namespace armarx::detail::VisitPointLike
 
namespace armarx
{
    inline void
    VisitPointLikeContainer(const auto& cloud, auto&& perElem, auto&& sizeInfo)
    {
        using handler = ::armarx::detail::VisitPointLike::VisitPointLike<decltype(cloud)>;
        static_assert(handler::value, "This type is not supported!");
        handler::visit(cloud, perElem, sizeInfo);
    }
} // namespace armarx