Skip to content

Instantly share code, notes, and snippets.

@facontidavide
Last active June 19, 2024 01:18
Show Gist options
  • Save facontidavide/13e9d0e395d193ad87cfd1213bdaaec1 to your computer and use it in GitHub Desktop.
Save facontidavide/13e9d0e395d193ad87cfd1213bdaaec1 to your computer and use it in GitHub Desktop.
Convert any point 3D from one type to another.
#include <type_traits>
// Magically converts any representation of a point in 3D
// (type with x, y and z) to another one. Works with:
//
// - pcl::PointXYZ, pcl::PointXYZI, pcl::PointXYZRGB, etc
// - Eigen::Vector3d, Eigen::Vector3f
// - custom type with x,y,z fields.
// - arrays or vectors with 3 elements.
template <class T, class = void>
struct type_has_method_x : std::false_type {};
template <class T>
struct type_has_method_x<T, std::void_t<decltype(T().x())>> : std::true_type {};
template <class T, class = void>
struct type_has_member_x : std::false_type {};
template <class T>
struct type_has_member_x<T, std::void_t<decltype(T::x)>> : std::true_type {};
template <class T, class = void>
struct type_has_operator : std::false_type {};
template <class T>
struct type_has_operator<T, std::void_t<decltype(T().operator[])>> : std::true_type{};
template <typename PointOut, typename PointIn>
inline PointOut ConvertPoint(const PointIn& v)
{
static_assert(std::is_same_v<PointIn, PointOut> ||
type_has_method_x<PointIn>::value ||
type_has_member_x<PointIn>::value ||
type_has_operator<PointIn>::value,
"Can't convert to the specified type");
if constexpr (std::is_same_v<PointIn, PointOut>) {
return v;
}
if constexpr (type_has_method_x<PointIn>::value) {
return { v.x(), v.y(), v.z() };
}
if constexpr (type_has_member_x<PointIn>::value) {
return { v.x, v.y, v.z };
}
if constexpr (type_has_operator<PointIn>::value) {
return { v[0], v[1], v[2] };
}
}
//------------- usage -------------
struct MyPoint3D {
double x,y,z;
};
pcl::PointXYZI p1;
auto p2 = ConvertTo<Eigen::Vector3d>(p1);
auto p3 = ConvertTo<pcl::PointXYZ>(p2);
auto p4 = ConvertTo<std::array<double,3>>(p3);
auto p5 = ConvertTo<MyPoint3D>(p4);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment