diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 3aeebe2db4148..0be983c455dce 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 6 # x, y, z and car, pedestrian, bicycle max_voxel_size: 40000 point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index 117722b72320c..53f21089b768d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -52,9 +53,10 @@ class PointpaintingFusionNode : public FusionNode class_names_; std::vector pointcloud_range; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + centerpoint::DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; bool out_of_scope(const DetectedObjects & obj); diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 3e44fddb7c5fd..d02a6484207b9 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -21,6 +21,7 @@ + @@ -74,6 +75,7 @@ + diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 4b1a351739fc5..0b8fcfb2840ba 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -47,7 +47,6 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -59,6 +58,13 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -240,11 +246,12 @@ void PointpaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte continue; } autoware_auto_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject( - box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + centerpoint::box3DToDetectedObject(box3d, class_names_, has_twist_, obj); output_obj_msg.objects.emplace_back(obj); } + detection_class_remapper_.mapClasses(output_obj_msg); + obj_pub_ptr_->publish(output_obj_msg); } diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index a67484b7353ea..09d777f810a63 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -123,6 +123,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ### centerpoint ### ament_auto_add_library(centerpoint_lib SHARED lib/centerpoint_trt.cpp + lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp lib/network/network_trt.cpp diff --git a/perception/lidar_centerpoint/config/aip_x2.param.yaml b/perception/lidar_centerpoint/config/aip_x2.param.yaml index 02979979954dc..3789f69a5856a 100644 --- a/perception/lidar_centerpoint/config/aip_x2.param.yaml +++ b/perception/lidar_centerpoint/config/aip_x2.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] rename_car_to_truck_and_bus: false point_feature_size: 4 max_voxel_size: 40000 diff --git a/perception/lidar_centerpoint/config/default.param.yaml b/perception/lidar_centerpoint/config/default.param.yaml index 0b23c48887b6f..c93b59ac0b8f9 100644 --- a/perception/lidar_centerpoint/config/default.param.yaml +++ b/perception/lidar_centerpoint/config/default.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 4 max_voxel_size: 40000 point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..ed378ffa44a70 --- /dev/null +++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp new file mode 100644 index 0000000000000..18a78f122e170 --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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 LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace centerpoint +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 66ffdfd400cfd..7477442a1060d 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -16,6 +16,7 @@ #define LIDAR_CENTERPOINT__NODE_HPP_ #include +#include #include #include #include @@ -49,9 +50,10 @@ class LidarCenterPointNode : public rclcpp::Node float score_threshold_{0.0}; std::vector class_names_; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; // debugger diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 8204cac708bad..69b75b614498e 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -31,8 +31,7 @@ namespace centerpoint { void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj); uint8_t getSemanticType(const std::string & class_name); diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 839300dbcf376..e3e532cc14fda 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -5,8 +5,9 @@ - - + + + @@ -22,5 +23,6 @@ + diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..147fb360d7c97 --- /dev/null +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -0,0 +1,70 @@ +// Copyright 2022 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 + +namespace centerpoint +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert(std::pow(std::sqrt(min_area_matrix.size()), 2) == std::sqrt(min_area_matrix.size())); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 57237609139e5..9f18dfc80974d 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -23,8 +23,7 @@ namespace centerpoint using Label = autoware_auto_perception_msgs::msg::ObjectClassification; void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj) { // TODO(yukke42): the value of classification confidence of DNN, not probability. @@ -41,17 +40,6 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_centerpoint"), "Unexpected label: UNKNOWN is set."); } - float l = box3d.length; - float w = box3d.width; - if (classification.label == Label::CAR && rename_car_to_truck_and_bus) { - // Note: object size is referred from multi_object_tracker - if ((w * l > 2.2 * 5.5) && (w * l <= 2.5 * 7.9)) { - classification.label = Label::TRUCK; - } else if (w * l > 2.5 * 7.9) { - classification.label = Label::BUS; - } - } - if (isCarLikeVehicleLabel(classification.label)) { obj.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index bda666e1c74c7..602d4651c6fb8 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -52,7 +52,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -64,6 +63,13 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -127,10 +133,12 @@ void LidarCenterPointNode::pointCloudCallback( output_msg.header = input_pointcloud_msg->header; for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); output_msg.objects.emplace_back(obj); } + detection_class_remapper_.mapClasses(output_msg); + if (objects_sub_count > 0) { objects_pub_->publish(output_msg); }