diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 353f797b0bee1..8a7f4c6107c67 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -159,6 +159,50 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_gtest(test_nms test/test_nms.cpp ) + ament_auto_add_gtest(test_voxel_generator + test/test_voxel_generator.cpp + ) + + add_executable(test_preprocess_kernel + test/test_preprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_preprocess_kernel PUBLIC + ${test_preprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_preprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_preprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + + add_executable(test_postprocess_kernel + test/test_postprocess_kernel.cpp + lib/utils.cpp + ) + + target_include_directories(test_postprocess_kernel PUBLIC + ${test_postprocess_kernel_SOURCE_DIR} + ) + + target_link_libraries(test_postprocess_kernel + centerpoint_cuda_lib + gtest + gtest_main + ) + + ament_add_test(test_postprocess_kernel + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "$" + ) + endif() else() diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 118e31f892d72..218aaee125c02 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -78,6 +78,11 @@ cudaError_t generateVoxels_random_launch( { dim3 blocks((points_size + 256 - 1) / 256); dim3 threads(256); + + if (blocks.x == 0) { + return cudaGetLastError(); + } + generateVoxels_random_kernel<<>>( points, points_size, min_x_range, max_x_range, min_y_range, max_y_range, min_z_range, max_z_range, pillar_x_size, pillar_y_size, pillar_z_size, grid_y_size, grid_x_size, mask, diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp new file mode 100644 index 0000000000000..030ec7d2921e3 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -0,0 +1,367 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_postprocess_kernel.hpp" + +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PostprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + constexpr std::size_t class_size{5}; + constexpr std::size_t point_feature_size{4}; + constexpr std::size_t max_voxel_size{100000000}; + const std::vector point_cloud_range{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + const std::vector voxel_size{0.32, 0.32, 10.0}; + constexpr std::size_t downsample_factor{1}; + constexpr std::size_t encoder_in_feature_size{9}; + constexpr float score_threshold{0.35f}; + constexpr float circle_nms_dist_threshold{0.5f}; + const std::vector yaw_norm_thresholds{0.5, 0.5, 0.5}; + constexpr bool has_variance{false}; + + config_ptr_ = std::make_unique( + class_size, point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds, has_variance); + + postprocess_cuda_ptr_ = std::make_unique(*config_ptr_); + + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + + head_out_heatmap_d_ = cuda::make_unique(grid_xy_size * config_ptr_->class_size_); + head_out_offset_d_ = + cuda::make_unique(grid_xy_size * config_ptr_->head_out_offset_size_); + head_out_z_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_z_size_); + head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_dim_size_); + head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_rot_size_); + head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_ptr_->head_out_vel_size_); + + std::vector heatmap_host_vector(grid_xy_size * config_ptr_->class_size_); + std::fill(heatmap_host_vector.begin(), heatmap_host_vector.end(), -1e6); + cudaMemcpy( + head_out_heatmap_d_.get(), heatmap_host_vector.data(), + grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float), cudaMemcpyHostToDevice); + cudaMemset( + head_out_offset_d_.get(), 0, grid_xy_size * config_ptr_->head_out_offset_size_ * sizeof(float)); + cudaMemset(head_out_z_d_.get(), 0, grid_xy_size * config_ptr_->head_out_z_size_ * sizeof(float)); + cudaMemset( + head_out_dim_d_.get(), 0, grid_xy_size * config_ptr_->head_out_dim_size_ * sizeof(float)); + cudaMemset( + head_out_rot_d_.get(), 0, grid_xy_size * config_ptr_->head_out_rot_size_ * sizeof(float)); + cudaMemset( + head_out_vel_d_.get(), 0, grid_xy_size * config_ptr_->head_out_vel_size_ * sizeof(float)); +} + +void PostprocessKernelTest::TearDown() +{ +} + +TEST_F(PostprocessKernelTest, EmptyTensorTest) +{ + std::vector det_boxes3d; + + postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, SingleDetectionTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw = M_PI_4; + constexpr float detection_yaw_sin = std::sin(detection_yaw); + constexpr float detection_yaw_cos = std::sin(detection_yaw); + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); + + const auto det_box3d = det_boxes3d[0]; + EXPECT_EQ(detection_x, det_box3d.x); + EXPECT_EQ(detection_y, det_box3d.y); + EXPECT_EQ(detection_z, det_box3d.z); + EXPECT_NEAR(std::exp(detection_log_w), det_box3d.width, 1e-6); + EXPECT_NEAR(std::exp(detection_log_l), det_box3d.length, 1e-6); + EXPECT_NEAR(std::exp(detection_log_h), det_box3d.height, 1e-6); + EXPECT_EQ(detection_yaw, det_box3d.yaw); + EXPECT_EQ(detection_vel_x, det_box3d.vel_x); + EXPECT_EQ(detection_vel_y, det_box3d.vel_y); +} + +TEST_F(PostprocessKernelTest, InvalidYawTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_yaw_sin = 0.0; + constexpr float detection_yaw_cos = 0.2; + + const float idx = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index = config_ptr_->down_grid_size_x_ * std::floor(idy) + std::floor(idx); + const float detection_x_offset = idx - std::floor(idx); + const float detection_y_offset = idy - std::floor(idy); + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index], &detection_x_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index], &detection_y_offset, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index], &detection_yaw_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index], &detection_yaw_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(0, det_boxes3d.size()); +} + +TEST_F(PostprocessKernelTest, CircleNMSTest) +{ + std::vector det_boxes3d; + + constexpr float detection_x = 70.f; + constexpr float detection_y = -38.4f; + constexpr float detection_z = 1.0; + constexpr float detection_log_w = std::log(7.0); + constexpr float detection_log_l = std::log(1.0); + constexpr float detection_log_h = std::log(2.0); + constexpr float detection_yaw1_sin = 0.0; + constexpr float detection_yaw1_cos = 1.0; + constexpr float detection_yaw2_sin = 1.0; + constexpr float detection_yaw2_cos = 0.0; + constexpr float detection_vel_x = 5.0; + constexpr float detection_vel_y = -5.0; + + const float idx1 = + ((detection_x - config_ptr_->range_min_x_) / + (config_ptr_->voxel_size_x_ * config_ptr_->downsample_factor_)); + const float idy1 = + ((detection_y - config_ptr_->range_min_y_) / + (config_ptr_->voxel_size_y_ * config_ptr_->downsample_factor_)); + const std::size_t index1 = config_ptr_->down_grid_size_x_ * std::floor(idy1) + std::floor(idx1); + const float detection_x_offset1 = idx1 - std::floor(idx1); + const float detection_y_offset1 = idy1 - std::floor(idy1); + + const float idx2 = idx1 + 1.0; + const float idy2 = idy1 + 1.0; + const std::size_t index2 = config_ptr_->down_grid_size_x_ * std::floor(idy2) + std::floor(idx2); + const float detection_x_offset2 = detection_x_offset1 - 1.0; + const float detection_y_offset2 = detection_y_offset1 - 1.0; + + // Set the values in the cuda tensor + const auto grid_xy_size = config_ptr_->down_grid_size_x_ * config_ptr_->down_grid_size_y_; + float score = 1.f; + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index1], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index1], &detection_x_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index1], &detection_y_offset1, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index1], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index1], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index1], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index1], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index1], &detection_yaw1_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index1], &detection_yaw1_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index1], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index1], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_heatmap_d_[2 * grid_xy_size + index2], &score, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_offset_d_[0 * grid_xy_size + index2], &detection_x_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_offset_d_[1 * grid_xy_size + index2], &detection_y_offset2, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_z_d_[0 * grid_xy_size + index2], &detection_z, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_dim_d_[0 * grid_xy_size + index2], &detection_log_w, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[1 * grid_xy_size + index2], &detection_log_l, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_dim_d_[2 * grid_xy_size + index2], &detection_log_h, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_rot_d_[0 * grid_xy_size + index2], &detection_yaw2_cos, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_rot_d_[1 * grid_xy_size + index2], &detection_yaw2_sin, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + cudaMemcpy( + &head_out_vel_d_[0 * grid_xy_size + index2], &detection_vel_x, 1 * sizeof(float), + cudaMemcpyHostToDevice); + cudaMemcpy( + &head_out_vel_d_[1 * grid_xy_size + index2], &detection_vel_y, 1 * sizeof(float), + cudaMemcpyHostToDevice); + + auto code = cudaGetLastError(); + ASSERT_EQ(cudaSuccess, code); + + // Extract the boxes + code = postprocess_cuda_ptr_->generateDetectedBoxes3D_launch( + head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), + head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_); + + ASSERT_EQ(cudaSuccess, code); + ASSERT_EQ(1, det_boxes3d.size()); +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp new file mode 100644 index 0000000000000..d0c9123da9e77 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_POSTPROCESS_KERNEL_HPP_ +#define TEST_POSTPROCESS_KERNEL_HPP_ + +#include +#include + +#include + +#include + +namespace centerpoint +{ + +class PostprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::unique_ptr postprocess_cuda_ptr_; + std::unique_ptr config_ptr_; + + cuda::unique_ptr head_out_heatmap_d_; + cuda::unique_ptr head_out_offset_d_; + cuda::unique_ptr head_out_z_d_; + cuda::unique_ptr head_out_dim_d_; + cuda::unique_ptr head_out_rot_d_; + cuda::unique_ptr head_out_vel_d_; +}; + +} // namespace centerpoint + +#endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp new file mode 100644 index 0000000000000..06b9c53ddabfd --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp @@ -0,0 +1,402 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_preprocess_kernel.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +namespace centerpoint +{ + +void PreprocessKernelTest::SetUp() +{ + cudaStreamCreate(&stream_); + + max_voxel_size_ = 40000; + max_point_in_voxel_size_ = + 32; // this value is actually hardcoded in the kernel so it can not be changed + point_feature_size_ = 4; + point_dim_size_ = 3; + config_encoder_in_feature_size_ = 9; + encoder_out_feature_size_ = 32; + capacity_ = 1000000; + + range_min_x_ = -76.8f; + range_min_y_ = -76.8f; + range_min_z_ = -4.0f; + range_max_x_ = 76.8f; + range_max_y_ = 76.8f; + range_max_z_ = 6.0f; + voxel_size_x_ = 0.32f; + voxel_size_y_ = 0.32f; + voxel_size_z_ = 10.0f; + + grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); + grid_size_y_ = static_cast((range_max_y_ - range_min_y_) / voxel_size_y_); + grid_size_z_ = static_cast((range_max_z_ - range_min_z_) / voxel_size_z_); + + voxels_size_ = max_voxel_size_ * max_point_in_voxel_size_ * point_feature_size_; + coordinates_size_ = max_voxel_size_ * point_dim_size_; + encoder_in_feature_size_ = + max_voxel_size_ * max_point_in_voxel_size_ * config_encoder_in_feature_size_; + pillar_features_size_ = max_voxel_size_ * encoder_out_feature_size_; + spatial_features_size_ = grid_size_x_ * grid_size_y_ * encoder_out_feature_size_; + grid_xy_size_ = grid_size_x_ * grid_size_y_; + + voxels_buffer_size_ = + grid_size_x_ * grid_size_y_ * max_point_in_voxel_size_ * point_feature_size_; + mask_size_ = grid_size_x_ * grid_size_y_; + + voxels_d_ = cuda::make_unique(voxels_size_); + coordinates_d_ = cuda::make_unique(coordinates_size_); + num_points_per_voxel_d_ = cuda::make_unique(max_voxel_size_); + encoder_in_features_d_ = cuda::make_unique(encoder_in_feature_size_); + pillar_features_d_ = cuda::make_unique(pillar_features_size_); + spatial_features_d_ = cuda::make_unique(spatial_features_size_); + points_d_ = cuda::make_unique(capacity_ * point_feature_size_); + voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); + mask_d_ = cuda::make_unique(mask_size_); + num_voxels_d_ = cuda::make_unique(1); + + CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(mask_d_.get(), 0, mask_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(voxels_d_.get(), 0, voxels_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(coordinates_d_.get(), 0, coordinates_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(num_points_per_voxel_d_.get(), 0, max_voxel_size_ * sizeof(float), stream_)); +} + +void PreprocessKernelTest::TearDown() +{ +} + +TEST_F(PreprocessKernelTest, EmptyVoxelTest) +{ + std::vector points{}; + std::size_t points_num = 0; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, BasicTest) +{ + std::vector points{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(1, sum); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + unsigned voxel_count; + std::array voxel_data{0.f, 0.f, 0.f, 0.f}; + + CHECK_CUDA_ERROR(cudaMemcpy( + &voxel_count, mask_d_.get() + voxel_index, 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, voxel_count); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(points[0], voxel_data[0]); + EXPECT_EQ(points[1], voxel_data[1]); + EXPECT_EQ(points[2], voxel_data[2]); + EXPECT_EQ(points[3], voxel_data[3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars; + std::vector voxel_features(point_feature_size_); + float num_voxels; + std::vector voxel_coordinates(3); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(1.0, num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + EXPECT_EQ(points[0], voxel_features[0]); + EXPECT_EQ(points[1], voxel_features[1]); + EXPECT_EQ(points[2], voxel_features[2]); + EXPECT_EQ(points[3], voxel_features[3]); + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features(config_encoder_in_feature_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + config_encoder_in_feature_size_ * sizeof(float), cudaMemcpyDeviceToHost)); + + // The first four values are just the point features + EXPECT_EQ(points[0], encoder_features[0]); + EXPECT_EQ(points[1], encoder_features[1]); + EXPECT_EQ(points[2], encoder_features[2]); + EXPECT_EQ(points[3], encoder_features[3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_EQ(0.0, encoder_features[4]); + EXPECT_EQ(0.0, encoder_features[5]); + EXPECT_EQ(0.0, encoder_features[6]); + + // The last two values are the relative coordinates with respect to the voxel center + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + EXPECT_EQ(points[0] - voxel_x_offset, encoder_features[7]); + EXPECT_EQ(points[1] - voxel_y_offset, encoder_features[8]); +} + +TEST_F(PreprocessKernelTest, OutOfRangeTest) +{ + std::vector points{25.f, -61.1f, 100.f, 0.1f}; // 100.f is out of range + std::size_t points_num = 1; + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Compute the total amount of voxels filled + thrust::host_vector mask_h(mask_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + mask_h.data(), mask_d_.get(), mask_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + int sum = thrust::reduce(mask_h.begin(), mask_h.end(), 0); + EXPECT_EQ(0, sum); +} + +TEST_F(PreprocessKernelTest, VoxelOverflowTest) +{ + std::array point{25.f, -61.1f, 1.8f, 0.1f}; + std::size_t points_num = 64; + std::vector points{}; + + for (std::size_t i = 0; i < points_num; ++i) { + points.insert(points.end(), point.begin(), point.end()); + } + + CHECK_CUDA_ERROR(cudaMemcpy( + points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice)); + + // Note: due to atomic operations in the kernel, generateVoxels does not handle overflows in the + // counter, and instead is done in the following kernel + cudaError_t code = centerpoint::generateVoxels_random_launch( + points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, + range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, + grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + // Check that the voxel was filled + int voxel_idx = std::floor((points[0] - range_min_x_) / voxel_size_x_); + int voxel_idy = std::floor((points[1] - range_min_y_) / voxel_size_y_); + unsigned int voxel_index = voxel_idy * grid_size_x_ + voxel_idx; + + std::vector voxel_data{}; + voxel_data.resize((max_point_in_voxel_size_ + 1) * point_feature_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_data.data(), + voxels_buffer_d_.get() + voxel_index * max_point_in_voxel_size_ * point_feature_size_, + (max_point_in_voxel_size_ + 1) * point_feature_size_ * sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + + for (std::size_t i = 0; i < max_point_in_voxel_size_; ++i) { + EXPECT_EQ(points[0], voxel_data[i * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_data[i * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_data[i * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_data[i * point_feature_size_ + 3]); + } + + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 0]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 1]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 2]); + EXPECT_EQ(0.0, voxel_data[max_point_in_voxel_size_ * point_feature_size_ + 3]); + + code = generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), grid_size_y_, grid_size_x_, max_voxel_size_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_); + + ASSERT_EQ(cudaSuccess, code); + + unsigned int num_pillars; + std::vector voxel_features(max_point_in_voxel_size_ * point_feature_size_); + float num_voxels; + std::vector voxel_coordinates(3); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_pillars, num_voxels_d_.get(), 1 * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_features.data(), voxels_d_.get(), + max_point_in_voxel_size_ * point_feature_size_ * sizeof(unsigned int), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + &num_voxels, num_points_per_voxel_d_.get(), 1 * sizeof(float), cudaMemcpyDeviceToHost)); + + CHECK_CUDA_ERROR(cudaMemcpy( + voxel_coordinates.data(), coordinates_d_.get(), 3 * sizeof(int), cudaMemcpyDeviceToHost)); + + EXPECT_EQ(1, num_pillars); + EXPECT_EQ(static_cast(max_point_in_voxel_size_), num_voxels); + EXPECT_EQ(0, voxel_coordinates[0]); + EXPECT_EQ(voxel_idy, voxel_coordinates[1]); + EXPECT_EQ(voxel_idx, voxel_coordinates[2]); + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + EXPECT_EQ(points[0], voxel_features[point_index * point_feature_size_ + 0]); + EXPECT_EQ(points[1], voxel_features[point_index * point_feature_size_ + 1]); + EXPECT_EQ(points[2], voxel_features[point_index * point_feature_size_ + 2]); + EXPECT_EQ(points[3], voxel_features[point_index * point_feature_size_ + 3]); + } + + code = generateFeatures_launch( + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), + max_voxel_size_, voxel_size_x_, voxel_size_y_, voxel_size_z_, range_min_x_, range_min_y_, + range_min_z_, encoder_in_features_d_.get(), stream_); + + ASSERT_EQ(cudaSuccess, code); + + std::vector encoder_features(max_point_in_voxel_size_ * config_encoder_in_feature_size_); + + CHECK_CUDA_ERROR(cudaMemcpy( + encoder_features.data(), encoder_in_features_d_.get(), + max_point_in_voxel_size_ * config_encoder_in_feature_size_ * sizeof(float), + cudaMemcpyDeviceToHost)); + + float voxel_x_offset = voxel_size_x_ / 2 + voxel_idx * voxel_size_x_ + range_min_x_; + float voxel_y_offset = voxel_size_y_ / 2 + voxel_idy * voxel_size_y_ + range_min_y_; + + for (std::size_t point_index = 0; point_index < max_point_in_voxel_size_; ++point_index) { + // The first four values are just the point features + EXPECT_EQ( + points[point_index * point_feature_size_ + 0], + encoder_features[point_index * config_encoder_in_feature_size_ + 0]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1], + encoder_features[point_index * config_encoder_in_feature_size_ + 1]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 2], + encoder_features[point_index * config_encoder_in_feature_size_ + 2]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 3], + encoder_features[point_index * config_encoder_in_feature_size_ + 3]); + + // The next three values are the relative coordinates with respect to the voxel average + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 4], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 5], 1e-4); + EXPECT_NEAR(0.0, encoder_features[point_index * config_encoder_in_feature_size_ + 6], 1e-4); + + // The last two values are the relative coordinates with respect to the voxel center + EXPECT_EQ( + points[point_index * point_feature_size_ + 0] - voxel_x_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 7]); + EXPECT_EQ( + points[point_index * point_feature_size_ + 1] - voxel_y_offset, + encoder_features[point_index * config_encoder_in_feature_size_ + 8]); + } +} + +} // namespace centerpoint + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp new file mode 100644 index 0000000000000..57ff51966a417 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -0,0 +1,79 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PREPROCESS_KERNEL_HPP_ +#define TEST_PREPROCESS_KERNEL_HPP_ + +#include + +#include + +namespace centerpoint +{ + +class PreprocessKernelTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + cudaStream_t stream_{nullptr}; + + std::size_t max_voxel_size_{}; + std::size_t max_point_in_voxel_size_{}; + std::size_t point_feature_size_{}; + std::size_t point_dim_size_{}; + std::size_t config_encoder_in_feature_size_{}; + std::size_t encoder_out_feature_size_{}; + std::size_t capacity_{}; + + float range_min_x_{}; + float range_min_y_{}; + float range_min_z_{}; + float range_max_x_{}; + float range_max_y_{}; + float range_max_z_{}; + float voxel_size_x_{}; + float voxel_size_y_{}; + float voxel_size_z_{}; + + std::size_t grid_size_x_{}; + std::size_t grid_size_y_{}; + std::size_t grid_size_z_{}; + + std::size_t voxels_size_{}; + std::size_t coordinates_size_{}; + std::size_t encoder_in_feature_size_{}; + std::size_t pillar_features_size_{}; + std::size_t spatial_features_size_{}; + std::size_t grid_xy_size_{}; + + std::size_t voxels_buffer_size_{}; + std::size_t mask_size_{}; + + cuda::unique_ptr voxels_d_{}; + cuda::unique_ptr coordinates_d_{}; + cuda::unique_ptr num_points_per_voxel_d_{}; + cuda::unique_ptr encoder_in_features_d_{}; + cuda::unique_ptr pillar_features_d_{}; + cuda::unique_ptr spatial_features_d_{}; + cuda::unique_ptr points_d_{}; + cuda::unique_ptr voxels_buffer_d_{}; + cuda::unique_ptr mask_d_{}; + cuda::unique_ptr num_voxels_d_{}; +}; + +} // namespace centerpoint + +#endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/lidar_centerpoint/test/test_voxel_generator.cpp new file mode 100644 index 0000000000000..c49dc75e03438 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.cpp @@ -0,0 +1,230 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_voxel_generator.hpp" + +#include + +#include + +void VoxelGeneratorTest::SetUp() +{ + // Setup things that should occur before every test instance should go here + node_ = std::make_shared("voxel_generator_test_node"); + + world_frame_ = "map"; + lidar_frame_ = "lidar"; + points_per_pointcloud_ = 10; + capacity_ = 100; + delta_pointcloud_x_ = 1.0; + + class_size_ = 5; + point_feature_size_ = 4; + max_voxel_size_ = 100000000; + point_cloud_range_ = std::vector{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; + voxel_size_ = std::vector{0.32, 0.32, 10.0}; + downsample_factor_ = 1; + encoder_in_feature_size_ = 9; + score_threshold_ = 0.35f; + circle_nms_dist_threshold_ = 0.5f; + yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; + has_variance_ = false; + + cloud1_ = std::make_unique(); + cloud2_ = std::make_unique(); + + cloud1_->header.frame_id = lidar_frame_; + + // Set up the fields for x, y, and z coordinates + cloud1_->fields.resize(3); + sensor_msgs::PointCloud2Modifier modifier(*cloud1_); + modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(points_per_pointcloud_); + + // Create an iterator for the x, y, z fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud1_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud1_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud1_, "z"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + + *cloud2_ = *cloud1_; + + // Set the stamps for the point clouds. They usually come every 100ms + cloud1_->header.stamp.sec = 1; + cloud1_->header.stamp.nanosec = 100'000'000; + cloud2_->header.stamp.sec = 1; + cloud2_->header.stamp.nanosec = 200'000'000; + + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + + // The vehicle moves 1m/s in the x direction + const double world_origin_x = 6'371'000.0; + const double world_origin_y = 1'371'000.0; + + transform1_.header.stamp = cloud1_->header.stamp; + transform1_.header.frame_id = world_frame_; + transform1_.child_frame_id = lidar_frame_; + transform1_.transform.translation.x = world_origin_x; + transform1_.transform.translation.y = world_origin_y; + transform1_.transform.translation.z = 1.8; + transform1_.transform.rotation.x = 0.0; + transform1_.transform.rotation.y = 0.0; + transform1_.transform.rotation.z = 0.0; + transform1_.transform.rotation.w = 1.0; + + transform2_ = transform1_; + transform2_.header.stamp = cloud2_->header.stamp; + transform2_.transform.translation.x = world_origin_x + delta_pointcloud_x_; +} + +void VoxelGeneratorTest::TearDown() +{ +} + +TEST_F(VoxelGeneratorTest, SingleFrame) +{ + const unsigned int num_past_frames = 0; // only current frame + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_EQ(points_per_pointcloud_, generated_points_num); + + // Check valid points + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check invalid points + for (std::size_t i = points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +TEST_F(VoxelGeneratorTest, TwoFramesNoTf) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_FALSE(status1); + EXPECT_FALSE(status2); + EXPECT_EQ(0, generated_points_num); +} + +TEST_F(VoxelGeneratorTest, TwoFrames) +{ + const unsigned int num_past_frames = 1; + + centerpoint::DensificationParam param(world_frame_, num_past_frames); + + centerpoint::CenterPointConfig config( + class_size_, point_feature_size_, max_voxel_size_, point_cloud_range_, voxel_size_, + downsample_factor_, encoder_in_feature_size_, score_threshold_, circle_nms_dist_threshold_, + yaw_norm_thresholds_, has_variance_); + + centerpoint::VoxelGenerator voxel_generator(param, config); + std::vector points; + points.resize(capacity_ * config.point_feature_size_); + std::fill(points.begin(), points.end(), std::nan("")); + + tf2_buffer_->setTransform(transform1_, "authority1"); + tf2_buffer_->setTransform(transform2_, "authority1"); + + bool status1 = voxel_generator.enqueuePointCloud(*cloud1_, *tf2_buffer_); + bool status2 = voxel_generator.enqueuePointCloud(*cloud2_, *tf2_buffer_); + std::size_t generated_points_num = voxel_generator.generateSweepPoints(points); + + EXPECT_TRUE(status1); + EXPECT_TRUE(status2); + EXPECT_EQ(2 * points_per_pointcloud_, generated_points_num); + + // Check valid points for the latest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are no tf conversions + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 0]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 1]); + EXPECT_EQ(static_cast(i), points[i * config.point_feature_size_ + 2]); + EXPECT_EQ(static_cast(0), points[i * config.point_feature_size_ + 3]); + } + + // Check valid points forthe oldest pointcloud + for (std::size_t i = 0; i < points_per_pointcloud_; ++i) { + // There are tf conversions, so results are not numerically the same + EXPECT_NEAR( + static_cast(i) - delta_pointcloud_x_, + points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 0], 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 1], + 1e-6); + EXPECT_NEAR( + static_cast(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 2], + 1e-6); + EXPECT_NEAR(0.1, points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 3], 1e-6); + } + + // Check invalid points + for (std::size_t i = 2 * points_per_pointcloud_ * config.point_feature_size_; + i < capacity_ * config.point_feature_size_; ++i) { + EXPECT_TRUE(std::isnan(points[i])); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/lidar_centerpoint/test/test_voxel_generator.hpp new file mode 100644 index 0000000000000..8fb7d8dffa44d --- /dev/null +++ b/perception/lidar_centerpoint/test/test_voxel_generator.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_VOXEL_GENERATOR_HPP_ +#define TEST_VOXEL_GENERATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +class VoxelGeneratorTest : public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; + + // These need to be public so that they can be accessed in the test cases + rclcpp::Node::SharedPtr node_; + + std::unique_ptr cloud1_, cloud2_; + geometry_msgs::msg::TransformStamped transform1_, transform2_; + + std::unique_ptr densification_param_ptr_; + std::unique_ptr config_ptr_; + + std::unique_ptr tf2_buffer_; + + std::string world_frame_; + std::string lidar_frame_; + std::size_t points_per_pointcloud_; + std::size_t capacity_; + double delta_pointcloud_x_; + + std::size_t class_size_; + float point_feature_size_; + std::size_t max_voxel_size_; + + std::vector point_cloud_range_; + std::vector voxel_size_; + std::size_t downsample_factor_; + std::size_t encoder_in_feature_size_; + float score_threshold_; + float circle_nms_dist_threshold_; + std::vector yaw_norm_thresholds_; + bool has_variance_; +}; + +#endif // TEST_VOXEL_GENERATOR_HPP_