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

@@ -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

ouster-ros/src/point_cloud_compose.h

@@ -12,95 +12,150 @@
 
 namespace ouster_ros {
 
-namespace sensor = ouster::sensor;
+using ouster::sensor::ChanFieldType;
 
-template <sensor::ChanFieldType T>
-struct TypeSelector { /*undefined*/ };
+template <ChanFieldType T>
+struct TypeSelector { /*undefined*/
+};
 
 template <>
-struct TypeSelector<sensor::ChanFieldType::UINT8> { typedef uint8_t ElementType; };
+struct TypeSelector<ChanFieldType::UINT8> {
+    typedef uint8_t type;
+};
 
 template <>
-struct TypeSelector<sensor::ChanFieldType::UINT16> { typedef uint16_t ElementType; };
+struct TypeSelector<ChanFieldType::UINT16> {
+    typedef uint16_t type;
+};
 
 template <>
-struct TypeSelector<sensor::ChanFieldType::UINT32> { typedef uint32_t ElementType; };
+struct TypeSelector<ChanFieldType::UINT32> {
+    typedef uint32_t type;
+};
 
 template <>
-struct TypeSelector<sensor::ChanFieldType::UINT64> { typedef uint64_t ElementType; };
-
+struct TypeSelector<ChanFieldType::UINT64> {
+    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 <std::size_t Index, std::size_t N, const ChanFieldTable<N>& Table>
-constexpr auto compose_tuple_table() {
+constexpr auto make_lidar_scan_tuple() {
     if constexpr (Index < N) {
-        using ElementType = typename TypeSelector<Table[Index].second>::ElementType;
+        using ElementType = typename TypeSelector<Table[Index].second>::type;
         return std::tuple_cat(
-          std::make_tuple(static_cast<const ElementType*>(0)),
-          std::move(compose_tuple_table<Index + 1, N, Table>()));
+            std::make_tuple(static_cast<const ElementType*>(0)),
+            std::move(make_lidar_scan_tuple<Index + 1, N, Table>()));
     } else {
-       return std::make_tuple();
+        return std::make_tuple();
     }
 }
 
-template <std::size_t Index,  std::size_t N, const ChanFieldTable<N>& Table, typename Tuple>
-void map_scan_fields_to_tuple(Tuple& tuple, const ouster::LidarScan& ls) {
-    if constexpr (Index < std::tuple_size<Tuple>::value) {
-        using ElementType = typename TypeSelector<Table[Index].second>::ElementType;
-        std::get<Index>(tuple) = ls.field<ElementType>(Table[Index].first).data();
-        map_scan_fields_to_tuple<Index + 1, N, Table>(tuple, ls);
+/**
+ * @brief maps the fields of a LidarScan object to the elements of the supplied
+ * tuple in the same order.
+ */
+template <std::size_t Index, std::size_t N, const ChanFieldTable<N>& Table,
+          typename Tuple>
+void map_lidar_scan_fields_to_tuple(Tuple& tp, const ouster::LidarScan& ls) {
+    static_assert(
+        std::tuple_size_v<Tuple> == N,
+        "target tuple size has a different size from the channel field table");
+    if constexpr (Index < N) {
+        using FieldType = typename TypeSelector<Table[Index].second>::type;
+        using ElementType = std::remove_const_t<
+            std::remove_pointer_t<std::tuple_element_t<Index, Tuple>>>;
+        static_assert(std::is_same_v<ElementType, FieldType>,
+                      "tuple element, field element types mismatch!");
+        std::get<Index>(tp) = ls.field<FieldType>(Table[Index].first).data();
+        map_lidar_scan_fields_to_tuple<Index + 1, N, Table>(tp, ls);
     }
 }
 
-template <std::size_t Index,  std::size_t N, const ChanFieldTable<N>& Table, typename PointT, typename Tuple>
-void tuple_copy_values(PointT& point, Tuple& tuple, int src_idx) {
-    if constexpr (Index < N) {
-        point.template get<Index + 5>() = std::get<Index>(tuple)[src_idx];
-        tuple_copy_values<Index + 1, N, Table>(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 <std::size_t Index, std::size_t N, const ChanFieldTable<N>& 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 <std::size_t Index, typename PointT, typename Tuple>
+void copy_lidar_scan_fields_to_point(PointT& pt, const Tuple& tp, int idx) {
+    if constexpr (Index < std::tuple_size_v<Tuple>) {
+        point::get<5 + Index>(pt) = std::get<Index>(tp)[idx];
+        copy_lidar_scan_fields_to_point<Index + 1>(pt, tp, idx);
     } else {
-        unused_variable(src_idx);
+        unused_variable(pt);
+        unused_variable(tp);
+        unused_variable(idx);
     }
 }
 
 template <class T>
 using Cloud = pcl::PointCloud<T>;
 
-template <std::size_t N, const ChanFieldTable<N>& PROFILE, typename PointT, typename PointS>
-void scan_to_cloud_f_destaggered(
-    ouster_ros::Cloud<PointT>& cloud,
-    PointS& staging_point,
-    const ouster::PointsF& points,
-    uint64_t scan_ts, const ouster::LidarScan& ls,
-    const std::vector<int>& 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 <std::size_t N, const ChanFieldTable<N>& PROFILE, typename PointT,
+          typename PointS>
+void scan_to_cloud_f_destaggered(ouster_ros::Cloud<PointT>& cloud,
+                                 PointS& staging_point,
+                                 const ouster::PointsF& points,
+                                 uint64_t scan_ts, const ouster::LidarScan& ls,
+                                 const std::vector<int>& 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<std::is_same_v<PointT, PointS>>::run(
                 cloud.points[tgt_idx], staging_point);
             // all native point types have x, y, z, t and ring values
-            pt.x = static_cast<float>(xyz(0));
-            pt.y = static_cast<float>(xyz(1));
-            pt.z = static_cast<float>(xyz(2));
+            pt.x = static_cast<decltype(pt.x)>(xyz(0));
+            pt.y = static_cast<decltype(pt.y)>(xyz(1));
+            pt.z = static_cast<decltype(pt.z)>(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<uint32_t>(ts);
             pt.ring = static_cast<uint16_t>(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<!std::is_same_v<PointT, PointS>>::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<!std::is_same_v<PointT, PointS>>::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
+}  // namespace ouster_ros

ouster-ros/test/point_cloud_compose_test.cpp

@@ -0,0 +1,74 @@
+#include <gtest/gtest.h>
+#include <ouster/lidar_scan.h>
+
+// 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<std::remove_reference_t<decltype(p[0])>>(base + i);
+    };
+
+    auto range = ls.field<uint32_t>(RANGE);
+    auto signal = ls.field<uint16_t>(SIGNAL);
+    auto reflect = ls.field<uint8_t>(REFLECTIVITY);
+    auto near_ir = ls.field<uint16_t>(NEAR_IR);
+
+    // choose a base value that could ultimately wrap around
+    fill_data(range, static_cast<uint32_t>(1 + (1ULL << 32) - SAMPLES / 2),
+              SAMPLES);
+    fill_data(signal, static_cast<uint16_t>(3 + (1 << 16) - SAMPLES / 2),
+              SAMPLES);
+    fill_data(reflect, static_cast<uint8_t>(5 + (1 << 8) - SAMPLES / 2),
+              SAMPLES);
+    fill_data(near_ir, static_cast<uint16_t>(7 + (1 << 16) - SAMPLES / 2),
+              SAMPLES);
+
+    ouster_ros::Cloud<Point_RNG19_RFL8_SIG16_NIR16_DUAL> 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));
+    }
+}