From 25d9be1749925e4f72f8867575682300954c45ff Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 18 Oct 2023 08:04:56 -0700 Subject: [PATCH] Add a seed test module for the point_cloud_compose methods + Add description for the point_cloud_compose methods + refactor code and add compile time checks. --- ouster-ros/CMakeLists.txt | 1 + ouster-ros/src/point_cloud_compose.h | 151 +++++++++++++------ ouster-ros/test/point_cloud_compose_test.cpp | 74 +++++++++ 3 files changed, 178 insertions(+), 48 deletions(-) create mode 100644 ouster-ros/test/point_cloud_compose_test.cpp diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 84e5c2e0..541a976c 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -173,6 +173,7 @@ if(BUILD_TESTING) src/os_ros.cpp test/point_accessor_test.cpp test/point_transform_test.cpp + test/point_cloud_compose_test.cpp ) ament_target_dependencies(${PROJECT_NAME}_test rclcpp diff --git a/ouster-ros/src/point_cloud_compose.h b/ouster-ros/src/point_cloud_compose.h index 4ed3ef23..cd78e687 100644 --- a/ouster-ros/src/point_cloud_compose.h +++ b/ouster-ros/src/point_cloud_compose.h @@ -12,95 +12,150 @@ namespace ouster_ros { -namespace sensor = ouster::sensor; +using ouster::sensor::ChanFieldType; -template -struct TypeSelector { /*undefined*/ }; +template +struct TypeSelector { /*undefined*/ +}; template <> -struct TypeSelector { typedef uint8_t ElementType; }; +struct TypeSelector { + typedef uint8_t type; +}; template <> -struct TypeSelector { typedef uint16_t ElementType; }; +struct TypeSelector { + typedef uint16_t type; +}; template <> -struct TypeSelector { typedef uint32_t ElementType; }; +struct TypeSelector { + typedef uint32_t type; +}; template <> -struct TypeSelector { typedef uint64_t ElementType; }; - +struct TypeSelector { + typedef uint64_t type; +}; + +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable. + */ template & Table> -constexpr auto compose_tuple_table() { +constexpr auto make_lidar_scan_tuple() { if constexpr (Index < N) { - using ElementType = typename TypeSelector::ElementType; + using ElementType = typename TypeSelector::type; return std::tuple_cat( - std::make_tuple(static_cast(0)), - std::move(compose_tuple_table())); + std::make_tuple(static_cast(0)), + std::move(make_lidar_scan_tuple())); } else { - return std::make_tuple(); + return std::make_tuple(); } } -template & Table, typename Tuple> -void map_scan_fields_to_tuple(Tuple& tuple, const ouster::LidarScan& ls) { - if constexpr (Index < std::tuple_size::value) { - using ElementType = typename TypeSelector::ElementType; - std::get(tuple) = ls.field(Table[Index].first).data(); - map_scan_fields_to_tuple(tuple, ls); +/** + * @brief maps the fields of a LidarScan object to the elements of the supplied + * tuple in the same order. + */ +template & Table, + typename Tuple> +void map_lidar_scan_fields_to_tuple(Tuple& tp, const ouster::LidarScan& ls) { + static_assert( + std::tuple_size_v == N, + "target tuple size has a different size from the channel field table"); + if constexpr (Index < N) { + using FieldType = typename TypeSelector::type; + using ElementType = std::remove_const_t< + std::remove_pointer_t>>; + static_assert(std::is_same_v, + "tuple element, field element types mismatch!"); + std::get(tp) = ls.field(Table[Index].first).data(); + map_lidar_scan_fields_to_tuple(tp, ls); } } -template & Table, typename PointT, typename Tuple> -void tuple_copy_values(PointT& point, Tuple& tuple, int src_idx) { - if constexpr (Index < N) { - point.template get() = std::get(tuple)[src_idx]; - tuple_copy_values(point, tuple, src_idx); +/** + * @brief constructs a suitable tuple at compile time that can store a reference + * to all the fields of a specific LidarScan object (without conversion) + * according to the information specificed by the ChanFieldTable and directly + * maps the fields of the supplied LidarScan to the constructed tuple before + * returning. + * @param[in] ls LidarScan + */ +template & Table> +constexpr auto make_lidar_scan_tuple(const ouster::LidarScan& ls) { + auto tp = make_lidar_scan_tuple<0, N, Table>(); + map_lidar_scan_fields_to_tuple<0, N, Table>(tp, ls); + return tp; +} + +/** + * @brief copies field values from LidarScan fields combined as a tuple into the + * the corresponding elements of the input point pt. + * @param[in,out] pt + * @param[in] tp + * @param[in] idx index of the point to be copied. + * @remark this method is to be used mainly with sensor native point types. + */ +template +void copy_lidar_scan_fields_to_point(PointT& pt, const Tuple& tp, int idx) { + if constexpr (Index < std::tuple_size_v) { + point::get<5 + Index>(pt) = std::get(tp)[idx]; + copy_lidar_scan_fields_to_point(pt, tp, idx); } else { - unused_variable(src_idx); + unused_variable(pt); + unused_variable(tp); + unused_variable(idx); } } template using Cloud = pcl::PointCloud; -template & PROFILE, typename PointT, typename PointS> -void scan_to_cloud_f_destaggered( - ouster_ros::Cloud& cloud, - PointS& staging_point, - const ouster::PointsF& points, - uint64_t scan_ts, const ouster::LidarScan& ls, - const std::vector& pixel_shift_by_row) { - - auto profile_tuple = compose_tuple_table<0, N, PROFILE>(); - map_scan_fields_to_tuple<0, N, PROFILE>(profile_tuple, ls); +template & PROFILE, typename PointT, + typename PointS> +void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud, + PointS& staging_point, + const ouster::PointsF& points, + uint64_t scan_ts, const ouster::LidarScan& ls, + const std::vector& pixel_shift_by_row) { + auto ls_tuple = make_lidar_scan_tuple<0, N, PROFILE>(ls); auto timestamp = ls.timestamp(); for (auto u = 0; u < ls.h; u++) { for (auto v = 0; v < ls.w; v++) { const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w; - auto ts = timestamp[v_shift]; ts = ts > scan_ts ? ts - scan_ts : 0UL; + auto ts = timestamp[v_shift]; + ts = ts > scan_ts ? ts - scan_ts : 0UL; const auto src_idx = u * ls.w + v_shift; const auto tgt_idx = u * ls.w + v; const auto xyz = points.row(src_idx); - // if target point and staging point has matching type bind the target directly - // and avoid performing transform_point at the end + // if target point and staging point has matching type bind the + // target directly and avoid performing transform_point at the end auto& pt = CondBinaryBind>::run( cloud.points[tgt_idx], staging_point); // all native point types have x, y, z, t and ring values - pt.x = static_cast(xyz(0)); - pt.y = static_cast(xyz(1)); - pt.z = static_cast(xyz(2)); + pt.x = static_cast(xyz(0)); + pt.y = static_cast(xyz(1)); + pt.z = static_cast(xyz(2)); + // TODO: in the future we could probably skip copying t and ring + // values if knowning before hand that the target point cloud does + // not have a field to hold the timestamp or a ring for example the + // case of pcl::PointXYZ or pcl::PointXYZI. pt.t = static_cast(ts); pt.ring = static_cast(u); - tuple_copy_values<0, N, PROFILE>(pt, profile_tuple, src_idx); - // only perform point transform operation when PointT, and PointS don't match - CondBinaryOp>::run(cloud.points[tgt_idx], staging_point, + copy_lidar_scan_fields_to_point<0>(pt, ls_tuple, src_idx); + // only perform point transform operation when PointT, and PointS + // don't match + CondBinaryOp>::run( + cloud.points[tgt_idx], staging_point, [](auto& tgt_pt, const auto& src_pt) { - transform_point(tgt_pt, src_pt); - } - ); + point::transform(tgt_pt, src_pt); + }); } } } -} // namespace ouster_ros \ No newline at end of file +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/test/point_cloud_compose_test.cpp b/ouster-ros/test/point_cloud_compose_test.cpp new file mode 100644 index 00000000..c119f1bc --- /dev/null +++ b/ouster-ros/test/point_cloud_compose_test.cpp @@ -0,0 +1,74 @@ +#include +#include + +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the +// header file needs to be the first include due to PCL_NO_PRECOMPILE flag +// clang-format off +#include "ouster_ros/os_ros.h" +// clang-format on +#include "ouster_ros/sensor_point_types.h" +#include "ouster_ros/common_point_types.h" +#include "ouster_ros/os_point.h" +#include "../src/point_meta_helpers.h" +#include "../src/point_cloud_compose.h" + +class PointCloudComposeTest : public ::testing::Test { + protected: + void SetUp() override {} + + void TearDown() override {} +}; + +using namespace std; +using namespace ouster::sensor; +using namespace ouster_ros; + +// TODO: generalize the test case! + +TEST_F(PointCloudComposeTest, MapLidarScanFields) { + const auto WIDTH = 5U; + const auto HEIGHT = 3U; + const auto SAMPLES = WIDTH * HEIGHT; + UDPProfileLidar lidar_udp_profile = + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL; + + ouster::LidarScan ls(WIDTH, HEIGHT, lidar_udp_profile); + + auto fill_data = [](auto& img, auto base, auto count) { + auto* p = img.data(); + for (auto i = 0U; i < count; ++i) + p[i] = + static_cast>(base + i); + }; + + auto range = ls.field(RANGE); + auto signal = ls.field(SIGNAL); + auto reflect = ls.field(REFLECTIVITY); + auto near_ir = ls.field(NEAR_IR); + + // choose a base value that could ultimately wrap around + fill_data(range, static_cast(1 + (1ULL << 32) - SAMPLES / 2), + SAMPLES); + fill_data(signal, static_cast(3 + (1 << 16) - SAMPLES / 2), + SAMPLES); + fill_data(reflect, static_cast(5 + (1 << 8) - SAMPLES / 2), + SAMPLES); + fill_data(near_ir, static_cast(7 + (1 << 16) - SAMPLES / 2), + SAMPLES); + + ouster_ros::Cloud cloud{WIDTH, HEIGHT}; + + auto ls_tuple = + make_lidar_scan_tuple<0, Profile_RNG19_RFL8_SIG16_NIR16_DUAL.size(), + Profile_RNG19_RFL8_SIG16_NIR16_DUAL>(ls); + + ouster_ros::Point_RNG19_RFL8_SIG16_NIR16_DUAL pt; + + for (auto src_idx = 0U; src_idx < SAMPLES; ++src_idx) { + copy_lidar_scan_fields_to_point<0>(pt, ls_tuple, src_idx); + EXPECT_EQ(point::get<5>(pt), range(0, src_idx)); + EXPECT_EQ(point::get<6>(pt), signal(0, src_idx)); + EXPECT_EQ(point::get<7>(pt), reflect(0, src_idx)); + EXPECT_EQ(point::get<8>(pt), near_ir(0, src_idx)); + } +}