test(lidar_centerpoint): add test (#7029)

* test(lidar_centerpoint): add test Signed-off-by: kminoda <[email protected]> * update test Signed-off-by: kminoda <[email protected]> * update license Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * fix namespace Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · May 17, 2024 · 5105e83 · 5105e83
1 parent d836237
commit 5105e83
Show file tree

Hide file tree

Showing 4 changed files with 368 additions and 0 deletions.
diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt
@@ -147,6 +147,20 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     TARGETS centerpoint_cuda_lib
     DESTINATION lib
   )
+
+  if(BUILD_TESTING)
+    find_package(ament_cmake_gtest REQUIRED)
+    ament_auto_add_gtest(test_detection_class_remapper
+      test/test_detection_class_remapper.cpp
+    )
+    ament_auto_add_gtest(test_ros_utils
+      test/test_ros_utils.cpp
+    )
+    ament_auto_add_gtest(test_nms
+      test/test_nms.cpp
+    )
+  endif()
+
 else()
   find_package(ament_cmake_auto REQUIRED)
   ament_auto_find_build_dependencies()

diff --git a/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp b/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp
@@ -0,0 +1,92 @@
+// 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 <lidar_centerpoint/detection_class_remapper.hpp>
+
+#include <gtest/gtest.h>
+
+TEST(DetectionClassRemapperTest, MapClasses)
+{
+  centerpoint::DetectionClassRemapper remapper;
+
+  // Set up the parameters for the remapper
+  // Labels: CAR, TRUCK, TRAILER
+  std::vector<int64_t> allow_remapping_by_area_matrix = {
+    0, 0, 0,  // CAR cannot be remapped
+    0, 0, 1,  // TRUCK can be remapped to TRAILER
+    0, 1, 0   // TRAILER can be remapped to TRUCK
+  };
+  std::vector<double> min_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0};
+  std::vector<double> max_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 999.0, 0.0, 10.0, 0.0};
+
+  remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix);
+
+  // Create a DetectedObjects message with some objects
+  autoware_auto_perception_msgs::msg::DetectedObjects msg;
+
+  // CAR with area 4.0, which is out of the range for remapping
+  autoware_auto_perception_msgs::msg::DetectedObject obj1;
+  autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification;
+  obj1.shape.dimensions.x = 2.0;
+  obj1.shape.dimensions.y = 2.0;
+  obj1_classification.label = 0;
+  obj1_classification.probability = 1.0;
+  obj1.classification = {obj1_classification};
+  msg.objects.push_back(obj1);
+
+  // TRUCK with area 16.0, which is in the range for remapping to TRAILER
+  autoware_auto_perception_msgs::msg::DetectedObject obj2;
+  autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification;
+  obj2.shape.dimensions.x = 8.0;
+  obj2.shape.dimensions.y = 2.0;
+  obj2_classification.label = 1;
+  obj2_classification.probability = 1.0;
+  obj2.classification = {obj2_classification};
+  msg.objects.push_back(obj2);
+
+  // TRAILER with area 9.0, which is in the range for remapping to TRUCK
+  autoware_auto_perception_msgs::msg::DetectedObject obj3;
+  autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification;
+  obj3.shape.dimensions.x = 3.0;
+  obj3.shape.dimensions.y = 3.0;
+  obj3_classification.label = 2;
+  obj3_classification.probability = 1.0;
+  obj3.classification = {obj3_classification};
+  msg.objects.push_back(obj3);
+
+  // TRAILER with area 12.0, which is out of the range for remapping
+  autoware_auto_perception_msgs::msg::DetectedObject obj4;
+  autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification;
+  obj4.shape.dimensions.x = 4.0;
+  obj4.shape.dimensions.y = 3.0;
+  obj4_classification.label = 2;
+  obj4_classification.probability = 1.0;
+  obj4.classification = {obj4_classification};
+  msg.objects.push_back(obj4);
+
+  // Call the mapClasses function
+  remapper.mapClasses(msg);
+
+  // Check the remapped labels
+  EXPECT_EQ(msg.objects[0].classification[0].label, 0);
+  EXPECT_EQ(msg.objects[1].classification[0].label, 2);
+  EXPECT_EQ(msg.objects[2].classification[0].label, 1);
+  EXPECT_EQ(msg.objects[3].classification[0].label, 2);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/perception/lidar_centerpoint/test/test_nms.cpp b/perception/lidar_centerpoint/test/test_nms.cpp
@@ -0,0 +1,121 @@
+// 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 "lidar_centerpoint/postprocess/non_maximum_suppression.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(NonMaximumSuppressionTest, Apply)
+{
+  centerpoint::NonMaximumSuppression nms;
+  centerpoint::NMSParams params;
+  params.search_distance_2d_ = 1.0;
+  params.iou_threshold_ = 0.2;
+  params.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV;
+  params.target_class_names_ = {"CAR"};
+  nms.setParameters(params);
+
+  std::vector<centerpoint::DetectedObject> input_objects(4);
+
+  // Object 1
+  autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification;
+  obj1_classification.label = 0;  // Assuming "car" has label 0
+  obj1_classification.probability = 1.0;
+  input_objects[0].classification = {obj1_classification};  // Assuming "car" has label 0
+  input_objects[0].kinematics.pose_with_covariance.pose.position.x = 0.0;
+  input_objects[0].kinematics.pose_with_covariance.pose.position.y = 0.0;
+  input_objects[0].kinematics.pose_with_covariance.pose.orientation.x = 0.0;
+  input_objects[0].kinematics.pose_with_covariance.pose.orientation.y = 0.0;
+  input_objects[0].kinematics.pose_with_covariance.pose.orientation.z = 0.0;
+  input_objects[0].kinematics.pose_with_covariance.pose.orientation.w = 1.0;
+  input_objects[0].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
+  input_objects[0].shape.dimensions.x = 4.0;
+  input_objects[0].shape.dimensions.y = 2.0;
+
+  // Object 2 (overlaps with Object 1)
+  autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification;
+  obj2_classification.label = 0;  // Assuming "car" has label 0
+  obj2_classification.probability = 1.0;
+  input_objects[1].classification = {obj2_classification};  // Assuming "car" has label 0
+  input_objects[1].kinematics.pose_with_covariance.pose.position.x = 0.5;
+  input_objects[1].kinematics.pose_with_covariance.pose.position.y = 0.5;
+  input_objects[1].kinematics.pose_with_covariance.pose.orientation.x = 0.0;
+  input_objects[1].kinematics.pose_with_covariance.pose.orientation.y = 0.0;
+  input_objects[1].kinematics.pose_with_covariance.pose.orientation.z = 0.0;
+  input_objects[1].kinematics.pose_with_covariance.pose.orientation.w = 1.0;
+  input_objects[1].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
+  input_objects[1].shape.dimensions.x = 4.0;
+  input_objects[1].shape.dimensions.y = 2.0;
+
+  // Object 3
+  autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification;
+  obj3_classification.label = 0;  // Assuming "car" has label 0
+  obj3_classification.probability = 1.0;
+  input_objects[2].classification = {obj3_classification};  // Assuming "car" has label 0
+  input_objects[2].kinematics.pose_with_covariance.pose.position.x = 5.0;
+  input_objects[2].kinematics.pose_with_covariance.pose.position.y = 5.0;
+  input_objects[2].kinematics.pose_with_covariance.pose.orientation.x = 0.0;
+  input_objects[2].kinematics.pose_with_covariance.pose.orientation.y = 0.0;
+  input_objects[2].kinematics.pose_with_covariance.pose.orientation.z = 0.0;
+  input_objects[2].kinematics.pose_with_covariance.pose.orientation.w = 1.0;
+  input_objects[2].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
+  input_objects[2].shape.dimensions.x = 4.0;
+  input_objects[2].shape.dimensions.y = 2.0;
+
+  // Object 4 (different class)
+  autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification;
+  obj4_classification.label = 1;  // Assuming "pedestrian" has label 1
+  obj4_classification.probability = 1.0;
+  input_objects[3].classification = {obj4_classification};  // Assuming "pedestrian" has label 1
+  input_objects[3].kinematics.pose_with_covariance.pose.position.x = 0.0;
+  input_objects[3].kinematics.pose_with_covariance.pose.position.y = 0.0;
+  input_objects[3].kinematics.pose_with_covariance.pose.orientation.x = 0.0;
+  input_objects[3].kinematics.pose_with_covariance.pose.orientation.y = 0.0;
+  input_objects[3].kinematics.pose_with_covariance.pose.orientation.z = 0.0;
+  input_objects[3].kinematics.pose_with_covariance.pose.orientation.w = 1.0;
+  input_objects[3].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
+  input_objects[3].shape.dimensions.x = 0.5;
+  input_objects[3].shape.dimensions.y = 0.5;
+
+  std::vector<centerpoint::DetectedObject> output_objects = nms.apply(input_objects);
+
+  // Assert the expected number of output objects
+  EXPECT_EQ(output_objects.size(), 3);
+
+  // Assert that input_objects[2] is included in the output_objects
+  bool is_input_object_2_included = false;
+  for (const auto & output_object : output_objects) {
+    if (output_object == input_objects[2]) {
+      is_input_object_2_included = true;
+      break;
+    }
+  }
+  EXPECT_TRUE(is_input_object_2_included);
+
+  // Assert that input_objects[3] is included in the output_objects
+  bool is_input_object_3_included = false;
+  for (const auto & output_object : output_objects) {
+    if (output_object == input_objects[3]) {
+      is_input_object_3_included = true;
+      break;
+    }
+  }
+  EXPECT_TRUE(is_input_object_3_included);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/perception/lidar_centerpoint/test/test_ros_utils.cpp b/perception/lidar_centerpoint/test/test_ros_utils.cpp
@@ -0,0 +1,141 @@
+// 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 "lidar_centerpoint/ros_utils.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(TestSuite, box3DToDetectedObject)
+{
+  std::vector<std::string> class_names = {"CAR",     "TRUCK",     "BUS",       "TRAILER",
+                                          "BICYCLE", "MOTORBIKE", "PEDESTRIAN"};
+
+  // Test case 1: Test with valid label, has_twist=true, has_variance=true
+  {
+    centerpoint::Box3D box3d;
+    box3d.score = 0.8f;
+    box3d.label = 0;  // CAR
+    box3d.x = 1.0;
+    box3d.y = 2.0;
+    box3d.z = 3.0;
+    box3d.yaw = 0.5;
+    box3d.length = 4.0;
+    box3d.width = 2.0;
+    box3d.height = 1.5;
+    box3d.vel_x = 1.0;
+    box3d.vel_y = 0.5;
+    box3d.x_variance = 0.1;
+    box3d.y_variance = 0.2;
+    box3d.z_variance = 0.3;
+    box3d.yaw_variance = 0.4;
+    box3d.vel_x_variance = 0.5;
+    box3d.vel_y_variance = 0.6;
+
+    autoware_auto_perception_msgs::msg::DetectedObject obj;
+    centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj);
+
+    EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f);
+    EXPECT_EQ(
+      obj.classification[0].label, autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
+    EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0);
+    EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0);
+    EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0);
+    EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0);
+    EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0);
+    EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5);
+    EXPECT_TRUE(obj.kinematics.has_position_covariance);
+    EXPECT_TRUE(obj.kinematics.has_twist);
+    EXPECT_TRUE(obj.kinematics.has_twist_covariance);
+  }
+
+  // Test case 2: Test with invalid label, has_twist=false, has_variance=false
+  {
+    centerpoint::Box3D box3d;
+    box3d.score = 0.5f;
+    box3d.label = 10;  // Invalid
+
+    autoware_auto_perception_msgs::msg::DetectedObject obj;
+    centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj);
+
+    EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f);
+    EXPECT_EQ(
+      obj.classification[0].label,
+      autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
+    EXPECT_FALSE(obj.kinematics.has_position_covariance);
+    EXPECT_FALSE(obj.kinematics.has_twist);
+    EXPECT_FALSE(obj.kinematics.has_twist_covariance);
+  }
+}
+
+TEST(TestSuite, getSemanticType)
+{
+  EXPECT_EQ(
+    centerpoint::getSemanticType("CAR"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("TRUCK"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("BUS"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::BUS);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("TRAILER"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("BICYCLE"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("MOTORBIKE"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("PEDESTRIAN"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
+  EXPECT_EQ(
+    centerpoint::getSemanticType("UNKNOWN"),
+    autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
+}
+
+TEST(TestSuite, convertPoseCovarianceMatrix)
+{
+  centerpoint::Box3D box3d;
+  box3d.x_variance = 0.1;
+  box3d.y_variance = 0.2;
+  box3d.z_variance = 0.3;
+  box3d.yaw_variance = 0.4;
+
+  std::array<double, 36> pose_covariance = centerpoint::convertPoseCovarianceMatrix(box3d);
+
+  EXPECT_FLOAT_EQ(pose_covariance[0], 0.1);
+  EXPECT_FLOAT_EQ(pose_covariance[7], 0.2);
+  EXPECT_FLOAT_EQ(pose_covariance[14], 0.3);
+  EXPECT_FLOAT_EQ(pose_covariance[35], 0.4);
+}
+
+TEST(TestSuite, convertTwistCovarianceMatrix)
+{
+  centerpoint::Box3D box3d;
+  box3d.vel_x_variance = 0.1;
+  box3d.vel_y_variance = 0.2;
+
+  std::array<double, 36> twist_covariance = centerpoint::convertTwistCovarianceMatrix(box3d);
+
+  EXPECT_FLOAT_EQ(twist_covariance[0], 0.1);
+  EXPECT_FLOAT_EQ(twist_covariance[7], 0.2);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}