test(lidar_centerpoint): added gtest for the cuda related parts of centerpoint

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 "$<TARGET_FILE:test_preprocess_kernel>"
+    )
+
+    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 "$<TARGET_FILE:test_postprocess_kernel>"
+    )
+
   endif()
 
 else()

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<<<blocks, threads, 0, stream>>>(
     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,

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 <gtest/gtest.h>
+
+#include <cmath>
+#include <memory>
+#include <vector>
+
+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<double> point_cloud_range{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0};
+  const std::vector<double> 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<double> yaw_norm_thresholds{0.5, 0.5, 0.5};
+  constexpr bool has_variance{false};
+
+  config_ptr_ = std::make_unique<centerpoint::CenterPointConfig>(
+    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<PostProcessCUDA>(*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<float[]>(grid_xy_size * config_ptr_->class_size_);
+  head_out_offset_d_ =
+    cuda::make_unique<float[]>(grid_xy_size * config_ptr_->head_out_offset_size_);
+  head_out_z_d_ = cuda::make_unique<float[]>(grid_xy_size * config_ptr_->head_out_z_size_);
+  head_out_dim_d_ = cuda::make_unique<float[]>(grid_xy_size * config_ptr_->head_out_dim_size_);
+  head_out_rot_d_ = cuda::make_unique<float[]>(grid_xy_size * config_ptr_->head_out_rot_size_);
+  head_out_vel_d_ = cuda::make_unique<float[]>(grid_xy_size * config_ptr_->head_out_vel_size_);
+
+  std::vector<float> heatmap_host_vector(grid_xy_size * config_ptr_->class_size_, 0.f);
+  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<Box3D> 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<Box3D> 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<Box3D> 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<Box3D> 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();
+}

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 <lidar_centerpoint/cuda_utils.hpp>
+#include <lidar_centerpoint/postprocess/postprocess_kernel.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+namespace centerpoint
+{
+
+class PostprocessKernelTest : public testing::Test
+{
+public:
+  void SetUp() override;
+  void TearDown() override;
+
+  cudaStream_t stream_{nullptr};
+
+  std::unique_ptr<PostProcessCUDA> postprocess_cuda_ptr_;
+  std::unique_ptr<centerpoint::CenterPointConfig> config_ptr_;
+
+  cuda::unique_ptr<float[]> head_out_heatmap_d_;
+  cuda::unique_ptr<float[]> head_out_offset_d_;
+  cuda::unique_ptr<float[]> head_out_z_d_;
+  cuda::unique_ptr<float[]> head_out_dim_d_;
+  cuda::unique_ptr<float[]> head_out_rot_d_;
+  cuda::unique_ptr<float[]> head_out_vel_d_;
+};
+
+}  // namespace centerpoint
+
+#endif  // TEST_POSTPROCESS_KERNEL_HPP_

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 <lidar_centerpoint/cuda_utils.hpp>
+
+#include <gtest/gtest.h>
+
+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<float[]> voxels_d_{};
+  cuda::unique_ptr<int[]> coordinates_d_{};
+  cuda::unique_ptr<float[]> num_points_per_voxel_d_{};
+  cuda::unique_ptr<float[]> encoder_in_features_d_{};
+  cuda::unique_ptr<float[]> pillar_features_d_{};
+  cuda::unique_ptr<float[]> spatial_features_d_{};
+  cuda::unique_ptr<float[]> points_d_{};
+  cuda::unique_ptr<float[]> voxels_buffer_d_{};
+  cuda::unique_ptr<unsigned int[]> mask_d_{};
+  cuda::unique_ptr<unsigned int[]> num_voxels_d_{};
+};
+
+}  // namespace centerpoint
+
+#endif  // TEST_PREPROCESS_KERNEL_HPP_

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 <sensor_msgs/point_cloud2_iterator.hpp>
+
+#include <gtest/gtest.h>
+
+void VoxelGeneratorTest::SetUp()
+{
+  // Setup things that should occur before every test instance should go here
+  node_ = std::make_shared<rclcpp::Node>("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<double>{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0};
+  voxel_size_ = std::vector<double>{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<double>{0.5, 0.5, 0.5};
+  has_variance_ = false;
+
+  cloud1_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+  cloud2_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  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<float> iter_x(*cloud1_, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud1_, "y");
+  sensor_msgs::PointCloud2Iterator<float> 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<float>(i);
+    *iter_y = static_cast<float>(i);
+    *iter_z = static_cast<float>(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<tf2_ros::Buffer>(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<float> 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<double>(i), points[i * config.point_feature_size_ + 0]);
+    EXPECT_EQ(static_cast<double>(i), points[i * config.point_feature_size_ + 1]);
+    EXPECT_EQ(static_cast<double>(i), points[i * config.point_feature_size_ + 2]);
+    EXPECT_EQ(static_cast<double>(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<float> 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<float> 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<double>(i), points[i * config.point_feature_size_ + 0]);
+    EXPECT_EQ(static_cast<double>(i), points[i * config.point_feature_size_ + 1]);
+    EXPECT_EQ(static_cast<double>(i), points[i * config.point_feature_size_ + 2]);
+    EXPECT_EQ(static_cast<double>(0), points[i * config.point_feature_size_ + 3]);
+  }
+
+  // Check valid points for the 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<double>(i) - delta_pointcloud_x_,
+      points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 0], 1e-6);
+    EXPECT_NEAR(
+      static_cast<double>(i), points[(points_per_pointcloud_ + i) * config.point_feature_size_ + 1],
+      1e-6);
+    EXPECT_NEAR(
+      static_cast<double>(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();
+}

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 <lidar_centerpoint/preprocess/voxel_generator.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+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<sensor_msgs::msg::PointCloud2> cloud1_, cloud2_;
+  geometry_msgs::msg::TransformStamped transform1_, transform2_;
+
+  std::unique_ptr<centerpoint::DensificationParam> densification_param_ptr_;
+  std::unique_ptr<centerpoint::CenterPointConfig> config_ptr_;
+
+  std::unique_ptr<tf2_ros::Buffer> 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<double> point_cloud_range_;
+  std::vector<double> voxel_size_;
+  std::size_t downsample_factor_;
+  std::size_t encoder_in_feature_size_;
+  float score_threshold_;
+  float circle_nms_dist_threshold_;
+  std::vector<double> yaw_norm_thresholds_;
+  bool has_variance_;
+};
+
+#endif  // TEST_VOXEL_GENERATOR_HPP_