diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 50d2c96f..84e5c2e0 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -172,7 +172,7 @@ if(BUILD_TESTING) test/ring_buffer_test.cpp src/os_ros.cpp test/point_accessor_test.cpp - # test/point_transform_test.cpp + test/point_transform_test.cpp ) ament_target_dependencies(${PROJECT_NAME}_test rclcpp diff --git a/ouster-ros/src/point_meta_helpers.h b/ouster-ros/src/point_meta_helpers.h index 754ec791..d58a7298 100644 --- a/ouster-ros/src/point_meta_helpers.h +++ b/ouster-ros/src/point_meta_helpers.h @@ -20,37 +20,37 @@ template inline void unused_variable(const T&) {} template -constexpr inline std::size_t point_element_count(const T& point) { +inline constexpr std::size_t point_element_count(const T& point) { return std::tuple_size::value; } template <> -constexpr inline std::size_t point_element_count(const pcl::PointXYZ&) { return 3U; } +inline constexpr std::size_t point_element_count(const pcl::PointXYZ&) { return 3U; } template <> -constexpr inline std::size_t point_element_count(const pcl::PointXYZI&) { return 4U; } +inline constexpr std::size_t point_element_count(const pcl::PointXYZI&) { return 4U; } // generic accessor that avoid having to type template before get template -inline auto& point_element_get(T& point) { return point.template get(); } +inline constexpr auto& point_element_get(T& point) { return point.template get(); } // pcl::PointXYZ compile time element accessors template <> -inline auto& point_element_get<0, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.x; } +inline constexpr auto& point_element_get<0, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.x; } template <> -inline auto& point_element_get<1, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.y; } +inline constexpr auto& point_element_get<1, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.y; } template <> -inline auto& point_element_get<2, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.z; } +inline constexpr auto& point_element_get<2, pcl::PointXYZ>(pcl::PointXYZ& point) { return point.z; } // pcl::PointXYZI compile time element accessors template <> -inline auto& point_element_get<0, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.x; } +inline constexpr auto& point_element_get<0, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.x; } template <> -inline auto& point_element_get<1, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.y; } +inline constexpr auto& point_element_get<1, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.y; } template <> -inline auto& point_element_get<2, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.z; } +inline constexpr auto& point_element_get<2, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.z; } template <> -inline auto& point_element_get<3, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.intensity; } +inline constexpr auto& point_element_get<3, pcl::PointXYZI>(pcl::PointXYZI& point) { return point.intensity; } template constexpr void iterate_point(PointT& point, UnaryOp unary_op) { diff --git a/ouster-ros/test/point_transform_test.cpp b/ouster-ros/test/point_transform_test.cpp index 3c1b4e61..6cecab3c 100644 --- a/ouster-ros/test/point_transform_test.cpp +++ b/ouster-ros/test/point_transform_test.cpp @@ -12,46 +12,59 @@ using namespace ouster_ros; using namespace std; class PointTransformTest : public ::testing::Test { - template + template void initialize_point_elements_with_randoms(PointT& pt) { std::default_random_engine g; std::uniform_real_distribution d(0.0, 128.0); - iterate_point<0, point_element_count(pt)>(pt, [&g, &d](auto& value) { + iterate_point<0, pt_size>(pt, [&g, &d](auto& value) { using value_type = std::remove_reference_t; value = static_cast(d(g)); }); - }; + } protected: void SetUp() override { // pcl + velodyne point types - initialize_point_elements_with_randoms(pt_xyz); - initialize_point_elements_with_randoms(pt_xyzi); - initialize_point_elements_with_randoms(pt_xyzir); + initialize_point_elements_with_randoms(pt_xyz); + initialize_point_elements_with_randoms(pt_xyzi); + initialize_point_elements_with_randoms(pt_xyzir); // native sensor point types - initialize_point_elements_with_randoms(pt_legacy); - initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16_dual); - initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16); - initialize_point_elements_with_randoms(pt_rg15_rfl8_nr8); + initialize_point_elements_with_randoms(pt_legacy); + initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16_dual); + initialize_point_elements_with_randoms(pt_rg19_rf8_sg16_nr16); + initialize_point_elements_with_randoms(pt_rg15_rfl8_nr8); // ouster_ros original/legacy point type - initialize_point_elements_with_randoms(pt_os_point); + initialize_point_elements_with_randoms(pt_os_point); } void TearDown() override {} // pcl & velodyne point types - pcl::PointXYZ pt_xyz; - pcl::PointXYZI pt_xyzi; - PointXYZIR pt_xyzir; + static pcl::PointXYZ pt_xyz; + static pcl::PointXYZI pt_xyzi; + static PointXYZIR pt_xyzir; // native point types - Point_LEGACY pt_legacy; - Point_RNG19_RFL8_SIG16_NIR16_DUAL pt_rg19_rf8_sg16_nr16_dual; - Point_RNG19_RFL8_SIG16_NIR16 pt_rg19_rf8_sg16_nr16; - Point_RNG15_RFL8_NIR8 pt_rg15_rfl8_nr8; + static Point_LEGACY pt_legacy; + static Point_RNG19_RFL8_SIG16_NIR16_DUAL pt_rg19_rf8_sg16_nr16_dual; + static Point_RNG19_RFL8_SIG16_NIR16 pt_rg19_rf8_sg16_nr16; + static Point_RNG15_RFL8_NIR8 pt_rg15_rfl8_nr8; // ouster_ros original/legacy point (not to be confused with Point_LEGACY) - ouster_ros::Point pt_os_point; + static ouster_ros::Point pt_os_point; }; +// pcl & velodyne point types +pcl::PointXYZ PointTransformTest::pt_xyz; +pcl::PointXYZI PointTransformTest::pt_xyzi; +PointXYZIR PointTransformTest::pt_xyzir; +// native point types +Point_LEGACY PointTransformTest::pt_legacy; +Point_RNG19_RFL8_SIG16_NIR16_DUAL PointTransformTest::pt_rg19_rf8_sg16_nr16_dual; +Point_RNG19_RFL8_SIG16_NIR16 PointTransformTest::pt_rg19_rf8_sg16_nr16; +Point_RNG15_RFL8_NIR8 PointTransformTest::pt_rg15_rfl8_nr8; +// ouster_ros original/legacy point (not to be confused with Point_LEGACY) +ouster_ros::Point PointTransformTest::pt_os_point; + + template void expect_points_xyz_equal(PointT& point1, PointU& point2) { EXPECT_EQ(point1.x, point2.x); @@ -153,10 +166,10 @@ void verify_point_transform(PointTGT& tgt_pt, const PointSRC& src_pt) { } // lambda function to check that point elements other than xyz have been zeroed -template +template void expect_point_fields_zeros(PointT& pt) { // starting at 3 to skip xyz - iterate_point<3, point_element_count(pt)>(pt, [](auto value) { + iterate_point<3, pt_size>(pt, [](auto value) { EXPECT_EQ(value, static_cast(0)); }); }; @@ -164,32 +177,31 @@ void expect_point_fields_zeros(PointT& pt) { TEST_F(PointTransformTest, ExpectPointFieldZeroed) { transform_point(pt_xyzi, pt_xyz); expect_points_xyz_equal(pt_xyzi, pt_xyz); - expect_point_fields_zeros( - pt_xyzi); // equivalent to EXPECT_EQ(pt_xyzi.intensity, 0.0f); + expect_point_fields_zeros(pt_xyzi); transform_point(pt_xyzir, pt_xyz); expect_points_xyz_equal(pt_xyzir, pt_xyz); - expect_point_fields_zeros(pt_xyzir); + expect_point_fields_zeros(pt_xyzir); transform_point(pt_legacy, pt_xyz); expect_points_xyz_equal(pt_legacy, pt_xyz); - expect_point_fields_zeros(pt_legacy); + expect_point_fields_zeros(pt_legacy); transform_point(pt_rg19_rf8_sg16_nr16_dual, pt_xyz); expect_points_xyz_equal(pt_rg19_rf8_sg16_nr16_dual, pt_xyz); - expect_point_fields_zeros(pt_rg19_rf8_sg16_nr16_dual); + expect_point_fields_zeros(pt_rg19_rf8_sg16_nr16_dual); transform_point(pt_rg19_rf8_sg16_nr16, pt_xyz); expect_points_xyz_equal(pt_rg19_rf8_sg16_nr16, pt_xyz); - expect_point_fields_zeros(pt_rg19_rf8_sg16_nr16); + expect_point_fields_zeros(pt_rg19_rf8_sg16_nr16); transform_point(pt_rg15_rfl8_nr8, pt_xyz); expect_points_xyz_equal(pt_rg15_rfl8_nr8, pt_xyz); - expect_point_fields_zeros(pt_rg15_rfl8_nr8); + expect_point_fields_zeros(pt_rg15_rfl8_nr8); transform_point(pt_os_point, pt_xyz); expect_points_xyz_equal(pt_os_point, pt_xyz); - expect_point_fields_zeros(pt_os_point); + expect_point_fields_zeros(pt_os_point); } TEST_F(PointTransformTest, TestTransformReduce_LEGACY) {