Skip to content

Commit

Permalink
Wrap point meta functions with a namespace and use shorter names for …
Browse files Browse the repository at this point in the history 
…the functions +

Add a short describion to all functions +
Refactor unit tests + Add new ElementGetSet unit test
  • Loading branch information
@Samahu
Samahu committed Oct 18, 2023
1 parent 8ac6661 commit 6b2aa62
Show file tree
Hide file tree
Showing 4 changed files with 234 additions and 136 deletions.
128 changes: 82 additions & 46 deletions ouster-ros/src/point_meta_helpers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@

namespace ouster_ros {

/**
* @brief A macro that facilitate providing a compile time check if a struct has
* a member with a specific name
*/
#define DEFINE_MEMBER_CHECKER(member) \
template<typename T, typename = void> \
struct has_##member : std::false_type { }; \
Expand All @@ -28,88 +32,120 @@ namespace ouster_ros {
template<typename C> \
inline constexpr bool has_##member##_v = has_##member<C>::value;

/**
* @brief A template that can be used to silence unused varaiables warning.
*/
template<typename T> inline void unused_variable(const T&) {}

/**
* @brief A struct that allows enabling or disabling the execution of an operation
* using a compile time condition.
*/
template <bool Enable>
struct CondBinaryOp {
template <typename A, typename B, typename BinaryOp>
inline static void run(A& a, B& b, BinaryOp binary_op) {
if constexpr (Enable) {
binary_op(a, b);
} else {
unused_variable(a); unused_variable(b); unused_variable(binary_op);
}
}
};

/**
* @brief A struct that a reference to the first variable if the compile time
* condition was true and returns a reference to the second variable if condition
* was false.
*/
template <bool Enable>
struct CondBinaryBind {
template <typename A, typename B>
inline static auto& run(A& a, B& b) {
if constexpr (Enable) {
unused_variable(b);
return a;
} else {
unused_variable(a);
return b;
}
}
};

namespace point {

/**
* @brief A compile-time function to retrieve the number of elements that a
* certain pcl point type has
* @param[in] point a pcl point type
* @return the number of elements that a point has
*/
template <typename T>
inline constexpr std::size_t point_element_count(const T& point) {
inline constexpr std::size_t size(const T& point) {
return std::tuple_size<decltype(point.as_tuple())>::value;
}

template <>
inline constexpr std::size_t point_element_count<pcl::PointXYZ>(const pcl::PointXYZ&) { return 3U; }
inline constexpr std::size_t size<pcl::PointXYZ>(const pcl::PointXYZ&) { return 3U; }

template <>
inline constexpr std::size_t point_element_count<pcl::PointXYZI>(const pcl::PointXYZI&) { return 4U; }
inline constexpr std::size_t size<pcl::PointXYZI>(const pcl::PointXYZI&) { return 4U; }

// generic accessor that avoid having to type template before get
template <size_t I, typename T>
inline constexpr auto& point_element_get(T& point) { return point.template get<I>(); }
inline constexpr auto& get(T& point) { return point.template get<I>(); }

// pcl::PointXYZ compile time element accessors
template <>
inline constexpr auto& point_element_get<0, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.x; }
inline constexpr auto& get<0, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.x; }
template <>
inline constexpr auto& point_element_get<1, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.y; }
inline constexpr auto& get<1, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.y; }
template <>
inline constexpr auto& point_element_get<2, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.z; }
inline constexpr auto& get<2, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.z; }

// pcl::PointXYZI compile time element accessors
template <>
inline constexpr auto& point_element_get<0, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.x; }
inline constexpr auto& get<0, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.x; }
template <>
inline constexpr auto& point_element_get<1, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.y; }
inline constexpr auto& get<1, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.y; }
template <>
inline constexpr auto& point_element_get<2, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.z; }
inline constexpr auto& get<2, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.z; }
template <>
inline constexpr auto& point_element_get<3, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.intensity; }
inline constexpr auto& get<3, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.intensity; }

// Apply an lambda function to the elements of a point in sequence
// TODO: create a generalized vardiac templates of apply and enumerate functions

/**
* @brief Iterates the elements of a point (compile time) applying a lambda
* function to each element in sequence within the range [Index, N) where:
* `Index < N and N <= size(pt)`
*/
template <std::size_t Index, std::size_t N, typename PointT, typename UnaryOp>
constexpr void iterate_point(PointT& point, UnaryOp unary_op) {
constexpr void apply(PointT& pt, UnaryOp unary_op) {
if constexpr (Index < N) {
unary_op(point_element_get<Index>(point));
iterate_point<Index + 1, N, PointT, UnaryOp>(point, unary_op);
unary_op(get<Index>(pt));
apply<Index + 1, N, PointT, UnaryOp>(pt, unary_op);
} else {
unused_variable(unary_op);
}
}

// Apply an lambda function to the elements of a point in sequence passing
// in the index of the element as the first parameter along with each element
/**
* @brief Enumerates the elements of a point (compile time) applying a lambda
* function to each element in sequence within the range [Index, N) where:
* `Index < N and N <= size(pt)`
* The lambda function recieves the index of each element as the first parameter
* and a reference to the element as the second parameter
*/
template <std::size_t Index, std::size_t N, typename PointT, typename EnumOp>
constexpr void enumerate_point(PointT& point, EnumOp enum_op) {
constexpr void enumerate(PointT& pt, EnumOp enum_op) {
if constexpr (Index < N) {
enum_op(Index, point_element_get<Index>(point));
enumerate_point<Index + 1, N, PointT, EnumOp>(point, enum_op);
enum_op(Index, get<Index>(pt));
enumerate<Index + 1, N, PointT, EnumOp>(pt, enum_op);
} else {
unused_variable(enum_op);
}
}

template <bool Enable>
struct CondBinaryOp {
template <typename A, typename B, typename BinaryOp>
inline static void run(A& a, B& b, BinaryOp binary_op) {
if constexpr (Enable) {
binary_op(a, b);
} else {
unused_variable(a); unused_variable(b); unused_variable(binary_op);
}
}
};

template <bool Enable>
struct CondBinaryBind {
template <typename A, typename B>
inline static auto& run(A& a, B& b) {
if constexpr (Enable) {
unused_variable(b);
return a;
} else {
unused_variable(a);
return b;
}
}
};

} // point
} // ouster_ros
4 changes: 3 additions & 1 deletion ouster-ros/src/point_transform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include "point_meta_helpers.h"

namespace ouster_ros {
namespace point {

DEFINE_MEMBER_CHECKER(x);
DEFINE_MEMBER_CHECKER(y);
Expand All @@ -27,7 +28,7 @@ DEFINE_MEMBER_CHECKER(reflectivity);
DEFINE_MEMBER_CHECKER(near_ir);

template <typename PointTGT, typename PointSRC>
void transform_point(PointTGT& tgt_pt, const PointSRC& src_pt) {
void transform(PointTGT& tgt_pt, const PointSRC& src_pt) {
// NOTE: for now we assume all points have xyz component
tgt_pt.x = src_pt.x; tgt_pt.y = src_pt.y; tgt_pt.z = src_pt.z;

Expand Down Expand Up @@ -122,4 +123,5 @@ void transform_point(PointTGT& tgt_pt, const PointSRC& src_pt) {
);
}

} // point
} // ouster_ros
Loading

0 comments on commit 6b2aa62

Please sign in to comment.