Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 5 commits into from
May 30, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 44 additions & 0 deletions perception/lidar_centerpoint/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
Original file line number Diff line number Diff line change
@@ -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,
367 changes: 367 additions & 0 deletions perception/lidar_centerpoint/test/test_postprocess_kernel.cpp
Original file line number Diff line number Diff line change
@@ -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);
}

Check warning on line 178 in perception/lidar_centerpoint/test/test_postprocess_kernel.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

TEST:PostprocessKernelTest:SingleDetectionTest has 76 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

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());
}

Check warning on line 359 in perception/lidar_centerpoint/test/test_postprocess_kernel.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

TEST:PostprocessKernelTest:CircleNMSTest has 105 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

} // namespace centerpoint

int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
49 changes: 49 additions & 0 deletions perception/lidar_centerpoint/test/test_postprocess_kernel.hpp
Original file line number Diff line number Diff line change
@@ -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_
403 changes: 403 additions & 0 deletions perception/lidar_centerpoint/test/test_preprocess_kernel.cpp

Large diffs are not rendered by default.

79 changes: 79 additions & 0 deletions perception/lidar_centerpoint/test/test_preprocess_kernel.hpp
Original file line number Diff line number Diff line change
@@ -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_
230 changes: 230 additions & 0 deletions perception/lidar_centerpoint/test/test_voxel_generator.cpp
Original file line number Diff line number Diff line change
@@ -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();
}
66 changes: 66 additions & 0 deletions perception/lidar_centerpoint/test/test_voxel_generator.hpp
Original file line number Diff line number Diff line change
@@ -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_