diff --git a/perception/lidar_centerpoint/.gitignore b/perception/lidar_centerpoint/.gitignore new file mode 100644 index 0000000000000..82f0c3ac6d4d4 --- /dev/null +++ b/perception/lidar_centerpoint/.gitignore @@ -0,0 +1 @@ +/data/ diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt new file mode 100644 index 0000000000000..c1c99076a4b47 --- /dev/null +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -0,0 +1,174 @@ +cmake_minimum_required(VERSION 3.5) +project(lidar_centerpoint) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CUDA_VERBOSE OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) +else() + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +option(TORCH_AVAIL "Torch available" OFF) +if(CUDA_FOUND) + set(Torch_DIR /usr/local/libtorch/share/cmake/Torch) + find_package(Torch) + if(TORCH_FOUND) + if(CUDA_VERBOSE) + message(STATUS "TORCH_INCLUDE_DIRS: ${TORCH_INCLUDE_DIRS}") + message(STATUS "TORCH_LIBRARIES: ${TORCH_LIBRARIES}") + endif() + set(TORCH_AVAIL ON) + else() + message("Torch NOT FOUND") + set(TORCH_AVAIL OFF) + endif() +endif() + +if(TRT_AVAIL AND CUDA_AVAIL AND TORCH_AVAIL) +# Download trained models + find_program(GDOWN_AVAIL "gdown") + if(NOT GDOWN_AVAIL) + message("gdown: command not found. External files could not be downloaded.") + endif() + + set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) + execute_process(COMMAND mkdir -p ${DATA_PATH}) + + function(download FILE_NAME GFILE_ID FILE_HASH) + # https://drive.google.com/file/d/GFILE_ID/view + message(STATUS "Checking and downloading ${FILE_NAME}") + set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) + if(EXISTS ${FILE_PATH}) + file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) + if(NOT ${FILE_HASH} EQUAL ${EXISTING_FILE_HASH}) + message(STATUS "... file hash changes. Downloading now ...") + execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH}) + endif() + else() + message(STATUS "... file doesn't exists. Downloading now ...") + execute_process(COMMAND gdown --quiet https://drive.google.com//uc?id=${GFILE_ID} -O ${FILE_PATH}) + endif() + endfunction() + + # default model + download(pts_voxel_encoder_default.onnx 1_8OCQmrPm_R4ZVh70QsS9HZo6uGrlbgz 01b860612e497591c4375d90dff61ef7) + download(pts_voxel_encoder_default.pt 1RZ7cuDnI-RBrDiWe-2vEs16mR_z0e9Uo 33136caa97e3bcef2cf3e04bbc93d1e4) + download(pts_backbone_neck_head_default.onnx 1UxDyt8T-TMJS7Ujx-1vbbqGRfDbMUZg2 e23a8ad4ea440f923e44dbe072b070da) + download(pts_backbone_neck_head_default.pt 1toAhmOriX8bwVI-ohuas9_2EBZnltoXh eb0df29b30acf9c1082ac4490af0bbc5) + + # aip_x2 model + download(pts_voxel_encoder_aip_x2.onnx 1x-NAHQ3W0lbLmjJlrL6Nhvdq8yz6Ux0n 65eeb95c5e48ebfe6894146cdb48c160) + download(pts_voxel_encoder_aip_x2.pt 1jzKopAhXWjnEgo_v8rtYy0hQIayUE-oL 4db81ce8edc6571aa0afb1ae43ee72e9) + download(pts_backbone_neck_head_aip_x2.onnx 1l2fdIQcBWr3-6stVoNkudnL4OZaPqmNT a33c8910fd9c9c910b10904d3cd96717) + download(pts_backbone_neck_head_aip_x2.pt 18iOAlRsjvcWoUG9KiL1PlD7OY5mi9BSw 274fdf1580dd899e36c050c1366f1883) + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + include_directories( + lib/include + ${CUDA_INCLUDE_DIRS} + ${TORCH_INCLUDE_DIRS} + ) + + ### centerpoint ### + ament_auto_add_library(centerpoint SHARED + lib/src/pointcloud_densification.cpp + lib/src/voxel_generator.cpp + lib/src/centerpoint_trt.cpp + lib/src/tensorrt_wrapper.cpp + lib/src/network_trt.cpp + ) + + target_link_libraries(centerpoint + ${NVINFER} + ${NVONNXPARSER} + ${NVINFER_PLUGIN} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${TORCH_LIBRARIES} + ) + + ## node ## + ament_auto_add_library(lidar_centerpoint_component SHARED + src/node.cpp + ) + + target_link_libraries(lidar_centerpoint_component + centerpoint + ) + + rclcpp_components_register_node(lidar_centerpoint_component + PLUGIN "centerpoint::LidarCenterPointNode" + EXECUTABLE lidar_centerpoint_node + ) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + endif() + + ament_auto_package( + INSTALL_TO_SHARE + launch + data + config + ) +else() + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + endif() + ament_auto_package( + INSTALL_TO_SHARE + launch + ) +endif() diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md new file mode 100644 index 0000000000000..fd827e09615a1 --- /dev/null +++ b/perception/lidar_centerpoint/README.md @@ -0,0 +1,61 @@ +# CenterPoint TensorRT + +This is a 3D object detection implementation of CenterPoint supporting TensorRT inference. + +The object.existence_probability is stored the value of classification confidence of DNN, not probability. + +## Parameters + +### Input Topics + +| Name | Type | Description | +| ---------------- | ----------- | ------------------------------------ | +| input/pointcloud | PointCloud2 | Point Clouds (x, y, z and intensity) | + +### Output Topics + +| Name | Type | Description | +| ------------------------------ | ----------------------------- | ---------------------- | +| output/objects | DynamicObjectWithFeatureArray | 3D Bounding Box | +| debug/pointcloud_densification | PointCloud2 | multi-frame pointcloud | + +### ROS Parameters + +| Name | Type | Description | Default | +| ------------------------- | ------ | ----------------------------------------------------------- | ------- | +| score_threshold | float | detected objects with score less than threshold are ignored | `0.4` | +| densification_base_frame | string | the base frame id to fuse multi-frame pointcloud | `map` | +| densification_past_frames | int | the number of past frames to fuse with the current frame | `1` | +| use_encoder_trt | bool | use TensorRT VoxelFeatureEncoder | `false` | +| use_head_trt | bool | use TensorRT DetectionHead | `true` | +| trt_precision | string | TensorRT inference precision: `fp32` or `fp16` | `fp16` | +| encoder_onnx_path | string | path to VoxelFeatureEncoder ONNX file | | +| encoder_engine_path | string | path to VoxelFeatureEncoder TensorRT Engine file | | +| encoder_pt_path | string | path to VoxelFeatureEncoder TorchScript file | | +| head_onnx_path | string | path to DetectionHead ONNX file | | +| head_engine_path | string | path to DetectionHead TensorRT Engine file | | +| head_pt_path | string | path to DetectionHead TorchScript file | | + +## For Developers + +If you have an error like `'GOMP_4.5' not found`, replace the OpenMP library in libtorch. + +```bash +sudo apt install libgomp1 -y +rm /path/to/libtorch/lib/libgomp-75eea7e8.so.1 +ln -s /usr/lib/x86_64-linux-gnu/libgomp.so.1 /path/to/libtorch/lib/libgomp-75eea7e8.so.1 +``` + +## Reference + +Yin, Tianwei, Xingyi Zhou, and Philipp Krähenbühl. "Center-based 3d object detection and tracking." arXiv preprint arXiv:2006.11275 (2020). + +## Reference Repositories + +- +- +- +- +- +- +- diff --git a/perception/lidar_centerpoint/config/aip_x2.param.yaml b/perception/lidar_centerpoint/config/aip_x2.param.yaml new file mode 100644 index 0000000000000..5cc8408a36e38 --- /dev/null +++ b/perception/lidar_centerpoint/config/aip_x2.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] diff --git a/perception/lidar_centerpoint/config/default.param.yaml b/perception/lidar_centerpoint/config/default.param.yaml new file mode 100644 index 0000000000000..ec7c7ddfee862 --- /dev/null +++ b/perception/lidar_centerpoint/config/default.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] + rename_car_to_truck_and_bus: true diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp new file mode 100644 index 0000000000000..1deab78ec14cf --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -0,0 +1,75 @@ +// Copyright 2021 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__NODE_HPP_ +#define LIDAR_CENTERPOINT__NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace centerpoint +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +class LidarCenterPointNode : public rclcpp::Node +{ +public: + explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options); + +private: + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg); + + static uint8_t getSemanticType(const std::string & class_name); + static bool isCarLikeVehicleLabel(const uint8_t label); + + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + + float score_threshold_{0.0}; + std::string densification_base_frame_; + int densification_past_frames_{0}; + bool use_encoder_trt_{false}; + bool use_head_trt_{false}; + std::string trt_precision_; + + std::string encoder_onnx_path_; + std::string encoder_engine_path_; + std::string encoder_pt_path_; + std::string head_onnx_path_; + std::string head_engine_path_; + std::string head_pt_path_; + + std::vector class_names_; + bool rename_car_to_truck_and_bus_{false}; + + std::unique_ptr densification_ptr_{nullptr}; + std::unique_ptr detector_ptr_{nullptr}; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__NODE_HPP_ diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml new file mode 100644 index 0000000000000..82b98f69fc065 --- /dev/null +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp b/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp new file mode 100644 index 0000000000000..fab250e8059f1 --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/centerpoint_trt.hpp @@ -0,0 +1,110 @@ +// Copyright 2021 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 CENTERPOINT_TRT_HPP_ +#define CENTERPOINT_TRT_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace centerpoint +{ +class NetworkParam +{ +public: + NetworkParam( + std::string onnx_path, std::string engine_path, std::string pt_path, std::string trt_precision, + const bool use_trt) + : onnx_path_(std::move(onnx_path)), + engine_path_(std::move(engine_path)), + pt_path_(std::move(pt_path)), + trt_precision_(std::move(trt_precision)), + use_trt_(use_trt) + { + } + + std::string onnx_path() const { return onnx_path_; } + std::string engine_path() const { return engine_path_; } + std::string pt_path() const { return pt_path_; } + std::string trt_precision() const { return trt_precision_; } + bool use_trt() const { return use_trt_; } + +private: + std::string onnx_path_; + std::string engine_path_; + std::string pt_path_; + std::string trt_precision_; + bool use_trt_; +}; + +class CenterPointTRT +{ +public: + explicit CenterPointTRT( + const NetworkParam & encoder_param, const NetworkParam & head_param, bool verbose); + + ~CenterPointTRT(); + + std::vector detect(const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg); + +private: + bool initPtr(bool use_encoder_trt, bool use_head_trt); + + bool loadTorchScript(torch::jit::script::Module & module, const std::string & model_path); + + static at::Tensor createInputFeatures( + const at::Tensor & voxels, const at::Tensor & coords, const at::Tensor & voxel_num_points); + + static at::Tensor scatterPillarFeatures( + const at::Tensor & pillar_features, const at::Tensor & coordinates); + + at::Tensor generatePredictedBoxes(); + + std::unique_ptr vg_ptr_ = nullptr; + torch::jit::script::Module encoder_pt_; + torch::jit::script::Module head_pt_; + std::unique_ptr encoder_trt_ptr_ = nullptr; + std::unique_ptr head_trt_ptr_ = nullptr; + c10::Device device_ = torch::kCUDA; + cudaStream_t stream_ = nullptr; + + at::Tensor voxels_t_; + at::Tensor coordinates_t_; + at::Tensor num_points_per_voxel_t_; + at::Tensor output_pillar_feature_t_; + at::Tensor output_heatmap_t_; + at::Tensor output_offset_t_; + at::Tensor output_z_t_; + at::Tensor output_dim_t_; + at::Tensor output_rot_t_; + at::Tensor output_vel_t_; +}; + +} // namespace centerpoint + +#endif // CENTERPOINT_TRT_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/config.hpp b/perception/lidar_centerpoint/lib/include/config.hpp new file mode 100644 index 0000000000000..7f006beeff116 --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/config.hpp @@ -0,0 +1,66 @@ +// Copyright 2021 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 CONFIG_HPP_ +#define CONFIG_HPP_ + +namespace centerpoint +{ +class Config +{ +public: + // input params + constexpr static int num_class = 3; // car, bicycle and pedestrian + constexpr static int num_point_dims = 3; // x, y and z + constexpr static int num_point_features = 4; // x, y, z and timelag + constexpr static int max_num_points_per_voxel = 32; + constexpr static int max_num_voxels = 40000; + constexpr static float pointcloud_range_xmin = -89.6f; + constexpr static float pointcloud_range_ymin = -89.6f; + constexpr static float pointcloud_range_zmin = -3.0f; + constexpr static float pointcloud_range_xmax = 89.6f; + constexpr static float pointcloud_range_ymax = 89.6f; + constexpr static float pointcloud_range_zmax = 5.0f; + constexpr static float voxel_size_x = 0.32f; + constexpr static float voxel_size_y = 0.32f; + constexpr static float voxel_size_z = 8.0f; + // = (pointcloud_range_xmax - pointcloud_range_xmin) / voxel_size_x + constexpr static int grid_size_x = 560; + // = (pointcloud_range_ymax - pointcloud_range_ymin) / voxel_size_y + constexpr static int grid_size_y = 560; + // = (pointcloud_range_zmax - pointcloud_range_zmin) / voxel_size_z + constexpr static int grid_size_z = 1; + constexpr static float offset_x = -89.44; // = pointcloud_range_xmin + voxel_size_x / 2 + constexpr static float offset_y = -89.44; // = pointcloud_range_ymin + voxel_size_y / 2 + constexpr static float offset_z = 1.0f; // = pointcloud_range_zmin + voxel_size_z / 2 + + // output params + constexpr static int num_box_features = 11; // score, class, x, y, z, w, l, h, yaw, vel_x, vel_y + constexpr static int max_num_output_objects = 200; + + // network params + constexpr static int downsample_factor = 2; + constexpr static int num_encoder_input_features = 8; + constexpr static int num_encoder_output_features = 32; + constexpr static int num_output_features = 6; + constexpr static int num_output_offset_features = 2; + constexpr static int num_output_z_features = 1; + constexpr static int num_output_dim_features = 3; + constexpr static int num_output_rot_features = 2; + constexpr static int num_output_vel_features = 2; +}; + +} // namespace centerpoint + +#endif // CONFIG_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/cuda_utils.hpp b/perception/lidar_centerpoint/lib/include/cuda_utils.hpp new file mode 100644 index 0000000000000..eaaf68fc1db1b --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/cuda_utils.hpp @@ -0,0 +1,120 @@ +// Copyright 2020 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. +/* + * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* + * This code is licensed under CC0 1.0 Universal (Public Domain). + * You can use this without any limitation. + * https://creativecommons.org/publicdomain/zero/1.0/deed.en + */ + +#ifndef CUDA_UTILS_HPP_ +#define CUDA_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) + +namespace cuda +{ +inline void check_error(const ::cudaError_t e, const char * f, int n) +{ + if (e != ::cudaSuccess) { + std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw std::runtime_error{s.str()}; + } +} + +struct deleter +{ + void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } +}; + +template +using unique_ptr = std::unique_ptr; + +template +typename std::enable_if::value, cuda::unique_ptr>::type make_unique( + const std::size_t n) +{ + using U = typename std::remove_extent::type; + U * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); + return cuda::unique_ptr{p}; +} + +template +cuda::unique_ptr make_unique() +{ + T * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); + return cuda::unique_ptr{p}; +} + +constexpr size_t CUDA_ALIGN = 256; + +template +inline size_t get_size_aligned(size_t num_elem) +{ + size_t size = num_elem * sizeof(T); + size_t extra_align = 0; + if (size % CUDA_ALIGN != 0) { + extra_align = CUDA_ALIGN - size % CUDA_ALIGN; + } + return size + extra_align; +} + +template +inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_size) +{ + size_t size = get_size_aligned(num_elem); + if (size > workspace_size) { + throw std::runtime_error("Workspace is too small!"); + } + workspace_size -= size; + T * ptr = reinterpret_cast(workspace); + workspace = reinterpret_cast(reinterpret_cast(workspace) + size); + return ptr; +} + +} // namespace cuda + +#endif // CUDA_UTILS_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/heatmap_utils.hpp b/perception/lidar_centerpoint/lib/include/heatmap_utils.hpp new file mode 100644 index 0000000000000..3e9f2fd19784d --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/heatmap_utils.hpp @@ -0,0 +1,113 @@ +// Copyright 2021 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 HEATMAP_UTILS_HPP_ +#define HEATMAP_UTILS_HPP_ + +#include + +#include + +namespace centerpoint +{ +at::Tensor sigmoid_hm(const at::Tensor & heatmap) +{ + // heatmap (float): (batch_size, num_class, H, W) + + return torch::clamp(torch::sigmoid(heatmap), /*min=*/1e-6, /*max=*/1 - 1e-6); +} + +at::Tensor nms_hm(const at::Tensor & heatmap, const int kernel_size = 3) +{ + // heatmap (float): (B, C, H, W) + + at::Tensor heatmap_max = torch::max_pool2d( + heatmap, {kernel_size, kernel_size}, + /*stride=*/{1}, /*padding=*/{(kernel_size - 1) / 2}); + at::Tensor mask = heatmap_max == heatmap; + return heatmap * mask.to(heatmap.dtype()); +} + +at::Tensor gather_feature(const at::Tensor & feature, const at::Tensor & index) +{ + // feature (float): (batch_size, topk * num_class, 1) + // feature (int): (batch_size, topk) + + int channel = feature.sizes()[2]; + auto index_size = index.sizes(); + at::Tensor _index = index.unsqueeze(-1).expand({index_size[0], index_size[1], channel}); + at::Tensor _feature = feature.gather(1, _index); + return _feature; +} + +std::tuple select_topk( + const at::Tensor & heatmap_pred, const int k) +{ + // heatmap_pred: (batch_size, num_class, H, W) + + const auto dtype = heatmap_pred.dtype(); + const int batch_size = heatmap_pred.sizes()[0]; + const int cls = heatmap_pred.sizes()[1]; + const int width = heatmap_pred.sizes()[3]; + + // first select topk scores in all classes and batches + // [B, C, H, W] -> [B, C, H*W] + at::Tensor _heatmap_pred = heatmap_pred.view({batch_size, cls, -1}); + + // both in [B, C, K] + auto topk_all_tuple = _heatmap_pred.topk(k); + at::Tensor topk_scores_all = std::get<0>(topk_all_tuple); + at::Tensor topk_inds_all = std::get<1>(topk_all_tuple); + + at::Tensor topk_ys = topk_inds_all.to(dtype).floor_divide(width); + at::Tensor topk_xs = topk_inds_all.to(dtype).fmod(width); + + // select topk examples across channels + // [B, C, K] -> [B, C*K] + topk_scores_all = topk_scores_all.view({batch_size, -1}); + + // Both in [N, K] + auto topk_tuple = topk_scores_all.topk(k); + at::Tensor topk_scores = std::get<0>(topk_tuple); + at::Tensor topk_inds = std::get<1>(topk_tuple); + at::Tensor topk_clses = topk_inds.to(dtype).floor_divide(k); + + topk_inds_all = topk_inds_all.view({batch_size, -1, 1}); + topk_ys = topk_ys.view({batch_size, -1, 1}); + topk_xs = topk_xs.view({batch_size, -1, 1}); + + topk_inds_all = gather_feature(topk_inds_all, topk_inds).view({batch_size, k}); + topk_ys = gather_feature(topk_ys, topk_inds).view({batch_size, k}); + topk_xs = gather_feature(topk_xs, topk_inds).view({batch_size, k}); + + return std::make_tuple(topk_scores, topk_inds_all, topk_clses, topk_ys, topk_xs); +} + +at::Tensor select_point_of_interest(const at::Tensor & index, const at::Tensor & feature_map) +{ + // index: (batch_size, N) + // feature_map: (batch_size, num_features, H, W) + + const int batch_size = feature_map.sizes()[0]; + const int channel = feature_map.sizes()[1]; + at::Tensor _index = index.view({batch_size, -1, 1}).repeat({1, 1, channel}); + at::Tensor _feature_map = feature_map.permute({0, 2, 3, 1}).contiguous(); + _feature_map = _feature_map.view({batch_size, -1, channel}); + _feature_map = _feature_map.gather(1, _index); + return _feature_map; +} + +} // namespace centerpoint + +#endif // HEATMAP_UTILS_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/network_trt.hpp b/perception/lidar_centerpoint/lib/include/network_trt.hpp new file mode 100644 index 0000000000000..22b335dbc218a --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/network_trt.hpp @@ -0,0 +1,46 @@ +// Copyright 2021 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 NETWORK_TRT_HPP_ +#define NETWORK_TRT_HPP_ + +#include + +namespace centerpoint +{ +class VoxelEncoderTRT : public TensorRTWrapper +{ +public: + using TensorRTWrapper::TensorRTWrapper; + +protected: + bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) override; +}; + +class HeadTRT : public TensorRTWrapper +{ +public: + using TensorRTWrapper::TensorRTWrapper; + +protected: + bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) override; +}; + +} // namespace centerpoint + +#endif // NETWORK_TRT_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/pointcloud_densification.hpp b/perception/lidar_centerpoint/lib/include/pointcloud_densification.hpp new file mode 100644 index 0000000000000..f551ab03520a7 --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/pointcloud_densification.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 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 POINTCLOUD_DENSIFICATION_HPP_ +#define POINTCLOUD_DENSIFICATION_HPP_ + +#include +#include +#include + +#include +#include + +namespace centerpoint +{ +class PointCloudDensification +{ +public: + PointCloudDensification( + std::string base_frame_id, unsigned int pointcloud_cache_size, rclcpp::Clock::SharedPtr clock); + + sensor_msgs::msg::PointCloud2 stackPointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg); + +private: + geometry_msgs::msg::TransformStamped getTransformStamped( + const std::string & target_frame, const std::string & source_frame); + + static void setTimeLag(sensor_msgs::msg::PointCloud2 & pointcloud_msg, float diff_timestamp); + + std::string base_frame_id_; + unsigned int pointcloud_cache_size_ = 0; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::list pointcloud_cache_; + std::list matrix_past2base_cache_; +}; + +} // namespace centerpoint + +#endif // POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp new file mode 100644 index 0000000000000..87687c443c580 --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/tensorrt_wrapper.hpp @@ -0,0 +1,89 @@ +// Copyright 2021 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 TENSORRT_WRAPPER_HPP_ +#define TENSORRT_WRAPPER_HPP_ + +#include + +#include +#include +#include + +namespace centerpoint +{ +struct Deleter +{ + template + void operator()(T * obj) const + { + if (obj) { + obj->destroy(); + } + } +}; + +template +using unique_ptr = std::unique_ptr; + +class Logger : public nvinfer1::ILogger +{ +public: + explicit Logger(bool verbose) : verbose_(verbose) {} + + void log(Severity severity, const char * msg) noexcept override + { + if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { + std::cout << msg << std::endl; + } + } + +private: + bool verbose_{false}; +}; + +class TensorRTWrapper +{ +public: + explicit TensorRTWrapper(bool verbose); + + bool init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision); + + unique_ptr context_ = nullptr; + +protected: + virtual bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) = 0; + +private: + bool parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + size_t workspace_size = (1ULL << 30)); + + bool saveEngine(const std::string & engine_path); + + bool loadEngine(const std::string & engine_path); + + bool createContext(); + + Logger logger_; + unique_ptr runtime_ = nullptr; + unique_ptr engine_ = nullptr; +}; + +} // namespace centerpoint + +#endif // TENSORRT_WRAPPER_HPP_ diff --git a/perception/lidar_centerpoint/lib/include/voxel_generator.hpp b/perception/lidar_centerpoint/lib/include/voxel_generator.hpp new file mode 100644 index 0000000000000..dd703b5201e44 --- /dev/null +++ b/perception/lidar_centerpoint/lib/include/voxel_generator.hpp @@ -0,0 +1,51 @@ +// Copyright 2021 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 VOXEL_GENERATOR_HPP_ +#define VOXEL_GENERATOR_HPP_ + +#include + +#include + +#include + +namespace centerpoint +{ +class VoxelGeneratorTemplate +{ +public: + virtual int pointsToVoxels( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, at::Tensor & voxels, + at::Tensor & coordinates, at::Tensor & num_points_per_voxel) = 0; + +protected: + float pointcloud_range_[6] = {Config::pointcloud_range_xmin, Config::pointcloud_range_ymin, + Config::pointcloud_range_zmin, Config::pointcloud_range_xmax, + Config::pointcloud_range_ymax, Config::pointcloud_range_zmax}; + float voxel_size_[3] = {Config::voxel_size_x, Config::voxel_size_y, Config::voxel_size_z}; + int grid_size_[3] = {Config::grid_size_x, Config::grid_size_y, Config::grid_size_z}; +}; + +class VoxelGenerator : public VoxelGeneratorTemplate +{ +public: + int pointsToVoxels( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, at::Tensor & voxels, + at::Tensor & coordinates, at::Tensor & num_points_per_voxel) override; +}; + +} // namespace centerpoint + +#endif // VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp new file mode 100644 index 0000000000000..374e890d5c3f1 --- /dev/null +++ b/perception/lidar_centerpoint/lib/src/centerpoint_trt.cpp @@ -0,0 +1,305 @@ +// Copyright 2021 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#define DEBUG_NAN 0 + +namespace centerpoint +{ +using torch::indexing::Slice; + +CenterPointTRT::CenterPointTRT( + const NetworkParam & encoder_param, const NetworkParam & head_param, const bool verbose) +{ + vg_ptr_ = std::make_unique(); + + if (encoder_param.use_trt()) { + encoder_trt_ptr_ = std::make_unique(verbose); + encoder_trt_ptr_->init( + encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision()); + } else { + loadTorchScript(encoder_pt_, encoder_param.pt_path()); + } + + if (head_param.use_trt()) { + head_trt_ptr_ = std::make_unique(verbose); + head_trt_ptr_->init( + head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision()); + head_trt_ptr_->context_->setBindingDimensions( + 0, nvinfer1::Dims4( + 1, Config::num_encoder_output_features, Config::grid_size_y, Config::grid_size_x)); + } else { + loadTorchScript(head_pt_, head_param.pt_path()); + } + + initPtr(encoder_param.use_trt(), head_param.use_trt()); + + torch::set_num_threads(1); // disable CPU parallelization + + cudaStreamCreate(&stream_); +} + +CenterPointTRT::~CenterPointTRT() +{ + if (stream_) { + cudaStreamSynchronize(stream_); + cudaStreamDestroy(stream_); + } +} + +bool CenterPointTRT::initPtr(const bool use_encoder_trt, const bool use_head_trt) +{ + if (use_encoder_trt) { + output_pillar_feature_t_ = torch::zeros( + {Config::max_num_voxels, Config::num_encoder_output_features}, + torch::TensorOptions().device(device_).dtype(torch::kFloat)); + } + + if (use_head_trt) { + const int downsample_grid_x = + static_cast(static_cast(Config::grid_size_x) / Config::downsample_factor); + const int downsample_grid_y = + static_cast(static_cast(Config::grid_size_y) / Config::downsample_factor); + const int batch_size = 1; + auto torch_options = torch::TensorOptions().device(device_).dtype(torch::kFloat); + output_heatmap_t_ = torch::zeros( + {batch_size, Config::num_class, downsample_grid_y, downsample_grid_x}, torch_options); + output_offset_t_ = torch::zeros( + {batch_size, Config::num_output_offset_features, downsample_grid_y, downsample_grid_x}, + torch_options); + output_z_t_ = torch::zeros( + {batch_size, Config::num_output_z_features, downsample_grid_y, downsample_grid_x}, + torch_options); + output_dim_t_ = torch::zeros( + {batch_size, Config::num_output_dim_features, downsample_grid_y, downsample_grid_x}, + torch_options); + output_rot_t_ = torch::zeros( + {batch_size, Config::num_output_rot_features, downsample_grid_y, downsample_grid_x}, + torch_options); + output_vel_t_ = torch::zeros( + {batch_size, Config::num_output_vel_features, downsample_grid_y, downsample_grid_x}, + torch_options); + } + + return true; +} + +std::vector CenterPointTRT::detect( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg) +{ + voxels_t_ = torch::zeros( + {Config::max_num_voxels, Config::max_num_points_per_voxel, Config::num_point_features}, + torch::TensorOptions().device(torch::kCPU).dtype(torch::kFloat)); + coordinates_t_ = torch::zeros( + {Config::max_num_voxels, Config::num_point_dims}, + torch::TensorOptions().device(torch::kCPU).dtype(torch::kInt)); + num_points_per_voxel_t_ = torch::zeros( + {Config::max_num_voxels}, torch::TensorOptions().device(torch::kCPU).dtype(torch::kInt)); + + int num_voxels = vg_ptr_->pointsToVoxels( + input_pointcloud_msg, voxels_t_, coordinates_t_, num_points_per_voxel_t_); + // Note: unlike python implementation, no slicing by num_voxels + // .s.t voxels_t_ = voxels_t_[:num_voxels]. + // w/ slicing more GPU memories are allocated + + voxels_t_ = voxels_t_.to(device_); + coordinates_t_ = coordinates_t_.to(device_); + num_points_per_voxel_t_ = num_points_per_voxel_t_.to(device_); + at::Tensor input_features = + createInputFeatures(voxels_t_, coordinates_t_, num_points_per_voxel_t_); + + // Note: num_voxels <= max_num_voxels, so input_features[num_voxels:] are invalid features. + input_features.index_put_({Slice(num_voxels)}, 0); + + if (encoder_trt_ptr_ && encoder_trt_ptr_->context_) { + std::vector encoder_buffers{ + input_features.data_ptr(), output_pillar_feature_t_.data_ptr()}; + encoder_trt_ptr_->context_->setBindingDimensions( + 0, nvinfer1::Dims3( + Config::max_num_voxels, Config::max_num_points_per_voxel, + Config::num_encoder_input_features)); + encoder_trt_ptr_->context_->enqueueV2(encoder_buffers.data(), stream_, nullptr); + } else { + std::vector batch_input_features; + batch_input_features.emplace_back(input_features); + batch_input_features.emplace_back(num_points_per_voxel_t_); + batch_input_features.emplace_back(coordinates_t_); + { + torch::NoGradGuard no_grad; + output_pillar_feature_t_ = encoder_pt_.forward(batch_input_features).toTensor(); + } + } + + at::Tensor spatial_features = + scatterPillarFeatures(output_pillar_feature_t_, coordinates_t_.to(torch::kLong)); + + if (head_trt_ptr_ && head_trt_ptr_->context_) { + std::vector head_buffers = {spatial_features.data_ptr(), output_heatmap_t_.data_ptr(), + output_offset_t_.data_ptr(), output_z_t_.data_ptr(), + output_dim_t_.data_ptr(), output_rot_t_.data_ptr(), + output_vel_t_.data_ptr()}; + head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); + } else { + std::vector batch_spatial_features; + batch_spatial_features.emplace_back(spatial_features); + + { + torch::NoGradGuard no_grad; + auto pred_arr = head_pt_.forward(batch_spatial_features).toTuple()->elements(); + output_heatmap_t_ = pred_arr[0].toTensor(); + output_offset_t_ = pred_arr[1].toTensor(); + output_z_t_ = pred_arr[2].toTensor(); + output_dim_t_ = pred_arr[3].toTensor(); + output_rot_t_ = pred_arr[4].toTensor(); + output_vel_t_ = pred_arr[5].toTensor(); + } + } + + at::Tensor boxes3d = generatePredictedBoxes(); + std::vector boxes3d_vec = + std::vector(boxes3d.data_ptr(), boxes3d.data_ptr() + boxes3d.numel()); + + return boxes3d_vec; +} + +at::Tensor CenterPointTRT::createInputFeatures( + const at::Tensor & voxels, const at::Tensor & coords, const at::Tensor & voxel_num_points) +{ + // voxels (float): (num_pillars, num_max_points, num_point_features) + // coordinates (int): (num_pillars, num_point_dims) + // voxel_num_points (int): (num_pillars,) + + at::Tensor coords_f = coords.to(torch::kFloat); + at::Tensor voxel_num_points_f = voxel_num_points.to(torch::kFloat); + + at::Tensor points_mean = + voxels.slice(/*dim=*/2, /*start=*/0, /*end=*/3).sum({1}, /*keepdim=*/true) / + voxel_num_points_f.view({-1, 1, 1}); + at::Tensor cluster = voxels.slice(2, 0, 3) - points_mean; + + // Note: unlike python implementation, batch_index isn't used in coords, + at::Tensor center_x = + voxels.slice(2, 0, 1) - + (coords_f.slice(1, 2, 3).unsqueeze(2) * Config::voxel_size_x + Config::offset_x); + at::Tensor center_y = + voxels.slice(2, 1, 2) - + (coords_f.slice(1, 1, 2).unsqueeze(2) * Config::voxel_size_y + Config::offset_y); + at::Tensor input_features = torch::cat({voxels, cluster, center_x, center_y}, /*dim=*/2); + + // paddings_indicator + const size_t axis = 0; + const int voxel_cnt = input_features.sizes()[1]; + at::Tensor actual_num = voxel_num_points.unsqueeze(axis + 1); + at::Tensor max_num = + torch::arange( + voxel_cnt, torch::TensorOptions().dtype(torch::kInt32).device(actual_num.device())) + .view({1, -1}); + at::Tensor mask = actual_num.to(torch::kInt32) > max_num; + mask = mask.unsqueeze(-1).to(torch::kFloat); + input_features *= mask; + + return input_features; // (num_pillars, num_max_points, num_voxel_features) +} + +at::Tensor CenterPointTRT::scatterPillarFeatures( + const at::Tensor & pillar_features, const at::Tensor & coordinates) +{ + // pillar_features (float): (num_pillars, num_encoder_output_features) + // coordinates (float): (num_pillars, num_point_dims) + + at::Tensor spatial_feature = torch::zeros( + {Config::num_encoder_output_features, Config::grid_size_y * Config::grid_size_x}, + torch::TensorOptions().dtype(pillar_features.dtype()).device(pillar_features.device())); + auto index = coordinates.select(1, 1) * Config::grid_size_x + coordinates.select(1, 2); + spatial_feature.index_put_({"...", index}, pillar_features.t()); + + return spatial_feature.view({1 /*batch size*/, -1, Config::grid_size_y, Config::grid_size_x}) + .contiguous(); +} + +at::Tensor CenterPointTRT::generatePredictedBoxes() +{ + // output_heatmap (float): (batch_size, num_class, H, W) + // output_offset (float): (batch_size, num_offset_features, H, W) + // output_z (float): (batch_size, num_z_features, H, W) + // output_dim (float): (batch_size, num_dim_features, H, W) + // output_rot (float): (batch_size, num_rot_features, H, W) + // output_vel (float): (batch_size, num_vel_features, H, W) + + at::Tensor heatmap_pred = output_heatmap_t_.clone(); + heatmap_pred = sigmoid_hm(heatmap_pred); + heatmap_pred = nms_hm(heatmap_pred); + + auto topk_tuple = select_topk(heatmap_pred, Config::max_num_output_objects); + at::Tensor scores = std::get<0>(topk_tuple); + at::Tensor index = std::get<1>(topk_tuple); + at::Tensor classes = std::get<2>(topk_tuple); + at::Tensor ys = std::get<3>(topk_tuple); + at::Tensor xs = std::get<4>(topk_tuple); + + at::Tensor offset_poi = select_point_of_interest(index, output_offset_t_); + at::Tensor z_poi = select_point_of_interest(index, output_z_t_); + at::Tensor dim_poi = select_point_of_interest(index, output_dim_t_); + at::Tensor rot_poi = select_point_of_interest(index, output_rot_t_); + at::Tensor vel_poi = select_point_of_interest(index, output_vel_t_); + + at::Tensor x = Config::voxel_size_x * Config::downsample_factor * + (xs.view({1, -1, 1}) + offset_poi.slice(2, 0, 1)) + + Config::pointcloud_range_xmin; + at::Tensor y = Config::voxel_size_y * Config::downsample_factor * + (ys.view({1, -1, 1}) + offset_poi.slice(2, 1, 2)) + + Config::pointcloud_range_ymin; + dim_poi = torch::exp(dim_poi); + at::Tensor rot = torch::atan2(rot_poi.slice(2, 0, 1), rot_poi.slice(2, 1, 2)); + rot = -rot - autoware_utils::pi / 2; + + at::Tensor boxes3d = + torch::cat( + {scores.view({1, -1, 1}), classes.view({1, -1, 1}), x, y, z_poi, dim_poi, rot, vel_poi}, + /*dim=*/2) + .contiguous() + .to(torch::kCPU) + .to(torch::kFloat); + + return boxes3d; +} + +bool CenterPointTRT::loadTorchScript( + torch::jit::script::Module & module, const std::string & model_path) +{ + try { + module = torch::jit::load(model_path, device_); + module.eval(); + } catch (const c10::Error & e) { + std::cout << "LOADING ERROR: " << e.msg() << std::endl; + return false; + } + std::cout << "Loading from " << model_path << std::endl; + return true; +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/src/network_trt.cpp b/perception/lidar_centerpoint/lib/src/network_trt.cpp new file mode 100644 index 0000000000000..c2e6b492af5b7 --- /dev/null +++ b/perception/lidar_centerpoint/lib/src/network_trt.cpp @@ -0,0 +1,75 @@ +// Copyright 2021 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 +#include + +namespace centerpoint +{ +bool VoxelEncoderTRT::setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) +{ + auto profile = builder.createOptimizationProfile(); + auto input_name = network.getInput(0)->getName(); + auto in_dims = nvinfer1::Dims3( + Config::max_num_voxels, Config::max_num_points_per_voxel, Config::num_encoder_input_features); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kMIN, in_dims); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kOPT, in_dims); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kMAX, in_dims); + auto output_name = network.getOutput(0)->getName(); + auto out_dims = nvinfer1::Dims2(Config::max_num_voxels, Config::num_encoder_output_features); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kMIN, out_dims); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kOPT, out_dims); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kMAX, out_dims); + config.addOptimizationProfile(profile); + + return true; +} + +bool HeadTRT::setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) +{ + auto profile = builder.createOptimizationProfile(); + auto input_name = network.getInput(0)->getName(); + auto in_dims = nvinfer1::Dims4( + 1, Config::num_encoder_output_features, Config::grid_size_y, Config::grid_size_x); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kMIN, in_dims); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kOPT, in_dims); + profile->setDimensions(input_name, nvinfer1::OptProfileSelector::kMAX, in_dims); + + const int output_channels[Config::num_output_features] = { + Config::num_class, + Config::num_output_offset_features, + Config::num_output_z_features, + Config::num_output_dim_features, + Config::num_output_rot_features, + Config::num_output_vel_features}; + const int downsample_grid_y = + static_cast(static_cast(Config::grid_size_y) / Config::downsample_factor); + const int downsample_grid_x = + static_cast(static_cast(Config::grid_size_x) / Config::downsample_factor); + for (int ci = 0; ci < Config::num_output_features; ci++) { + auto output_name = network.getOutput(ci)->getName(); + auto out_dims = nvinfer1::Dims4(1, output_channels[ci], downsample_grid_y, downsample_grid_x); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kMIN, out_dims); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kOPT, out_dims); + profile->setDimensions(output_name, nvinfer1::OptProfileSelector::kMAX, out_dims); + } + config.addOptimizationProfile(profile); + return true; +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/src/pointcloud_densification.cpp b/perception/lidar_centerpoint/lib/src/pointcloud_densification.cpp new file mode 100644 index 0000000000000..ac9f18d9c6054 --- /dev/null +++ b/perception/lidar_centerpoint/lib/src/pointcloud_densification.cpp @@ -0,0 +1,122 @@ +// Copyright 2021 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 +#include + +#include +#include + +#include +#include + +namespace centerpoint +{ +PointCloudDensification::PointCloudDensification( + std::string base_frame_id, const unsigned int pointcloud_cache_size, + rclcpp::Clock::SharedPtr clock) +: base_frame_id_(std::move(base_frame_id)), + pointcloud_cache_size_(pointcloud_cache_size), + tf_buffer_(clock) +{ +} + +sensor_msgs::msg::PointCloud2 PointCloudDensification::stackPointCloud( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg) +{ + const double input_timestamp = rclcpp::Time(input_pointcloud_msg.header.stamp).seconds(); + + auto transform_base2frame = + getTransformStamped(input_pointcloud_msg.header.frame_id, base_frame_id_); + Eigen::Matrix4f matrix_base2current = + tf2::transformToEigen(transform_base2frame.transform).matrix().cast(); + + sensor_msgs::msg::PointCloud2 output_pointcloud_msg; + output_pointcloud_msg = input_pointcloud_msg; + setTimeLag(output_pointcloud_msg, 0); + + // concat the current frame and past frames + auto pc_msg_iter = pointcloud_cache_.begin(); + auto matrix_iter = matrix_past2base_cache_.begin(); + for (; pc_msg_iter != pointcloud_cache_.end(); pc_msg_iter++, matrix_iter++) { + sensor_msgs::msg::PointCloud2 cached_pointcloud_msg = *pc_msg_iter; + sensor_msgs::msg::PointCloud2 transformed_pointcloud_msg; + + Eigen::Affine3f affine_past2base; + affine_past2base.matrix() = *matrix_iter; + Eigen::Affine3f affine_base2current; + affine_base2current.matrix() = matrix_base2current; + + pcl_ros::transformPointCloud( + (affine_base2current * affine_past2base).matrix(), cached_pointcloud_msg, + transformed_pointcloud_msg); + double diff_timestamp = + input_timestamp - rclcpp::Time(cached_pointcloud_msg.header.stamp).seconds(); + setTimeLag(transformed_pointcloud_msg, static_cast(diff_timestamp)); + + sensor_msgs::msg::PointCloud2 tmp_pointcloud_msg = output_pointcloud_msg; + pcl::concatenatePointCloud( + tmp_pointcloud_msg, transformed_pointcloud_msg, output_pointcloud_msg); + } + + // add input pointcloud to cache + auto transform_frame2base = + getTransformStamped(base_frame_id_, input_pointcloud_msg.header.frame_id); + Eigen::Matrix4f matrix_past2base = + tf2::transformToEigen(transform_frame2base.transform).matrix().cast(); + pointcloud_cache_.push_front(input_pointcloud_msg); + matrix_past2base_cache_.push_front(matrix_past2base); + if (pointcloud_cache_.size() > pointcloud_cache_size_) { + pointcloud_cache_.pop_back(); + matrix_past2base_cache_.pop_back(); + } + + return output_pointcloud_msg; +} + +geometry_msgs::msg::TransformStamped PointCloudDensification::getTransformStamped( + const std::string & target_frame, const std::string & source_frame) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform(target_frame, source_frame, rclcpp::Time(0)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(rclcpp::get_logger("PointCloudDensification"), "%s", ex.what()); + return transform; + } + return transform; +} + +void PointCloudDensification::setTimeLag( + sensor_msgs::msg::PointCloud2 & pointcloud_msg, const float diff_timestamp) +{ + // Note: pcl::fromROSMsg is very slow, so point values in ros message are changed directly. + // Note: intensity isn't used in this CenterPoint implementation. + + const int intensity_idx = pcl::getFieldIndex(pointcloud_msg, "intensity"); + if (intensity_idx == -1) { + return; + } + + const int width = pointcloud_msg.width; + const int point_step = pointcloud_msg.point_step; + const int offset = pointcloud_msg.fields[intensity_idx].offset; + for (int i = 0; i < width; i++) { + memcpy( + &pointcloud_msg.data[offset + point_step * i], + reinterpret_cast(&diff_timestamp), sizeof(float)); + } +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp new file mode 100644 index 0000000000000..118c1829c9250 --- /dev/null +++ b/perception/lidar_centerpoint/lib/src/tensorrt_wrapper.cpp @@ -0,0 +1,147 @@ +// Copyright 2021 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 + +#include + +#include +#include +#include + +namespace centerpoint +{ +TensorRTWrapper::TensorRTWrapper(bool verbose) : logger_(Logger(verbose)) {} + +bool TensorRTWrapper::init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision) +{ + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); + if (!runtime_) { + std::cout << "Fail to create runtime" << std::endl; + return false; + } + + bool success; + std::ifstream engine_file(engine_path); + if (engine_file.is_open()) { + success = loadEngine(engine_path); + } else { + success = parseONNX(onnx_path, engine_path, precision); + } + success &= createContext(); + + return success; +} + +bool TensorRTWrapper::createContext() +{ + if (!engine_) { + std::cout << "Fail to create context: Engine isn't created" << std::endl; + return false; + } + + context_ = unique_ptr(engine_->createExecutionContext()); + if (!context_) { + std::cout << "Fail to create context" << std::endl; + return false; + } + + return true; +} + +bool TensorRTWrapper::parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + const size_t workspace_size) +{ + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + std::cout << "Fail to create builder" << std::endl; + return false; + } + + auto config = unique_ptr(builder->createBuilderConfig()); + if (!config) { + std::cout << "Fail to create config" << std::endl; + return false; + } + config->setMaxWorkspaceSize(workspace_size); + if (precision == "fp16") { + if (builder->platformHasFastFp16()) { + std::cout << "use TensorRT FP16 Inference" << std::endl; + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } else { + std::cout << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; + } + } + + const auto flag = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = unique_ptr(builder->createNetworkV2(flag)); + if (!network) { + std::cout << "Fail to create network" << std::endl; + return false; + } + + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); + parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); + + if (!setProfile(*builder, *network, *config)) { + std::cout << "Fail to set profile" << std::endl; + return false; + } + + std::cout << "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..." + << std::endl; + engine_ = unique_ptr(builder->buildEngineWithConfig(*network, *config)); + if (!engine_) { + std::cout << "Fail to create engine" << std::endl; + return false; + } + + return saveEngine(engine_path); +} + +bool TensorRTWrapper::saveEngine(const std::string & engine_path) +{ + std::cout << "Writing to " << engine_path << std::endl; + auto serialized = unique_ptr(engine_->serialize()); + std::ofstream file(engine_path, std::ios::out | std::ios::binary); + file.write(reinterpret_cast(serialized->data()), serialized->size()); + return true; +} + +bool TensorRTWrapper::loadEngine(const std::string & engine_path) +{ + std::ifstream file(engine_path, std::ios::in | std::ios::binary); + file.seekg(0, std::ifstream::end); + const size_t size = file.tellg(); + file.seekg(0, std::ifstream::beg); + + std::unique_ptr buffer{new char[size]}; + file.read(buffer.get(), size); + file.close(); + + if (!runtime_) { + std::cout << "Fail to load engine: Runtime isn't created" << std::endl; + return false; + } + + std::cout << "Loading from " << engine_path << std::endl; + engine_ = + unique_ptr(runtime_->deserializeCudaEngine(buffer.get(), size, nullptr)); + return true; +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/src/voxel_generator.cpp b/perception/lidar_centerpoint/lib/src/voxel_generator.cpp new file mode 100644 index 0000000000000..2ec01be06ff6d --- /dev/null +++ b/perception/lidar_centerpoint/lib/src/voxel_generator.cpp @@ -0,0 +1,99 @@ +// Copyright 2021 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 + +#include + +#include + +namespace centerpoint +{ +int VoxelGenerator::pointsToVoxels( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, at::Tensor & voxels, + at::Tensor & coordinates, at::Tensor & num_points_per_voxel) +{ + // voxels (float): (max_num_voxels, max_num_points_per_voxel, num_point_features) + // coordinates (int): (max_num_voxels, num_point_dims) + // num_points_per_voxel (int): (max_num_voxels) + + at::Tensor coord_to_voxel_idx = torch::full( + {Config::grid_size_z, Config::grid_size_y, Config::grid_size_x}, -1, + at::TensorOptions().dtype(torch::kInt)); + + auto voxels_p = voxels.data_ptr(); + auto coordinates_p = coordinates.data_ptr(); + auto num_points_per_voxel_p = num_points_per_voxel.data_ptr(); + auto coord_to_voxel_idx_p = coord_to_voxel_idx.data_ptr(); + + int voxel_cnt = 0; // @return + float recip_voxel_size[3] = { + 1 / Config::voxel_size_x, 1 / Config::voxel_size_y, 1 / Config::voxel_size_z}; + float point[Config::num_point_features]; + int coord_zyx[Config::num_point_dims]; + bool out_of_range; + int c, coord_idx, voxel_idx, point_cnt; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud_msg, "x"), + iter_y(pointcloud_msg, "y"), iter_z(pointcloud_msg, "z"), + iter_i(pointcloud_msg, "intensity"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_i) { + point[0] = *iter_x; + point[1] = *iter_y; + point[2] = *iter_z; + point[3] = *iter_i; + + out_of_range = false; + for (int di = 0; di < Config::num_point_dims; di++) { + c = static_cast((point[di] - pointcloud_range_[di]) * recip_voxel_size[di]); + if (c < 0 || c >= grid_size_[di]) { + out_of_range = true; + break; + } + coord_zyx[Config::num_point_dims - di - 1] = c; + } + if (out_of_range) { + continue; + } + + coord_idx = coord_zyx[0] * Config::grid_size_y * Config::grid_size_x + + coord_zyx[1] * Config::grid_size_x + coord_zyx[2]; + voxel_idx = coord_to_voxel_idx_p[coord_idx]; + if (voxel_idx == -1) { + voxel_idx = voxel_cnt; + if (voxel_cnt >= Config::max_num_voxels) { + continue; + } + + voxel_cnt++; + coord_to_voxel_idx_p[coord_idx] = voxel_idx; + for (int di = 0; di < Config::num_point_dims; di++) { + coordinates_p[voxel_idx * Config::num_point_dims + di] = coord_zyx[di]; + } + } + + point_cnt = num_points_per_voxel_p[voxel_idx]; + if (point_cnt < Config::max_num_points_per_voxel) { + for (int fi = 0; fi < Config::num_point_features; fi++) { + voxels_p + [voxel_idx * Config::max_num_points_per_voxel * Config::num_point_features + + point_cnt * Config::num_point_features + fi] = point[fi]; + } + num_points_per_voxel_p[voxel_idx]++; + } + } + + return voxel_cnt; +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml new file mode 100644 index 0000000000000..23009ea141acf --- /dev/null +++ b/perception/lidar_centerpoint/package.xml @@ -0,0 +1,26 @@ + + + lidar_centerpoint + 1.0.0 + The lidar_centerpoint package + + Yusuke Muramatsu + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_utils + pcl_ros + rclcpp + rclcpp_components + tf2_geometry_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp new file mode 100644 index 0000000000000..d3b2328dbb6d9 --- /dev/null +++ b/perception/lidar_centerpoint/src/node.cpp @@ -0,0 +1,173 @@ +// Copyright 2021 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/node.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace centerpoint +{ +LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) +: Node("lidar_center_point", node_options) +{ + score_threshold_ = this->declare_parameter("score_threshold", 0.4); + densification_base_frame_ = this->declare_parameter("densification_base_frame", "map"); + densification_past_frames_ = this->declare_parameter("densification_past_frames", 1); + use_encoder_trt_ = this->declare_parameter("use_encoder_trt", false); + use_head_trt_ = this->declare_parameter("use_head_trt", true); + trt_precision_ = this->declare_parameter("trt_precision", "fp16"); + encoder_onnx_path_ = this->declare_parameter("encoder_onnx_path", ""); + encoder_engine_path_ = this->declare_parameter("encoder_engine_path", ""); + encoder_pt_path_ = this->declare_parameter("encoder_pt_path", ""); + head_onnx_path_ = this->declare_parameter("head_onnx_path", ""); + head_engine_path_ = this->declare_parameter("head_engine_path", ""); + head_pt_path_ = this->declare_parameter("head_pt_path", ""); + class_names_ = this->declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); + + NetworkParam encoder_param( + encoder_onnx_path_, encoder_engine_path_, encoder_pt_path_, trt_precision_, use_encoder_trt_); + NetworkParam head_param( + head_onnx_path_, head_engine_path_, head_pt_path_, trt_precision_, use_head_trt_); + densification_ptr_ = std::make_unique( + densification_base_frame_, densification_past_frames_, this->get_clock()); + detector_ptr_ = std::make_unique(encoder_param, head_param, /*verbose=*/false); + + pointcloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), + std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1)); + objects_pub_ = this->create_publisher( + "~/output/objects", rclcpp::QoS{1}); + pointcloud_pub_ = this->create_publisher( + "~/debug/pointcloud_densification", rclcpp::SensorDataQoS{}.keep_last(1)); +} + +void LidarCenterPointNode::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg) +{ + const auto objects_sub_count = + objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); + const auto pointcloud_sub_count = pointcloud_pub_->get_subscription_count() + + pointcloud_pub_->get_intra_process_subscription_count(); + if (objects_sub_count < 1 && pointcloud_sub_count < 1) { + return; + } + + auto stacked_pointcloud_msg = densification_ptr_->stackPointCloud(*input_pointcloud_msg); + std::vector boxes3d_vec = detector_ptr_->detect(stacked_pointcloud_msg); + + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = input_pointcloud_msg->header; + for (size_t obj_i = 0; obj_i < boxes3d_vec.size() / Config::num_box_features; obj_i++) { + float score = boxes3d_vec[obj_i * Config::num_box_features + 0]; + if (score < score_threshold_) { + continue; + } + + int class_id = static_cast(boxes3d_vec[obj_i * Config::num_box_features + 1]); + float x = boxes3d_vec[obj_i * Config::num_box_features + 2]; + float y = boxes3d_vec[obj_i * Config::num_box_features + 3]; + float z = boxes3d_vec[obj_i * Config::num_box_features + 4]; + float w = boxes3d_vec[obj_i * Config::num_box_features + 5]; + float l = boxes3d_vec[obj_i * Config::num_box_features + 6]; + float h = boxes3d_vec[obj_i * Config::num_box_features + 7]; + float yaw = boxes3d_vec[obj_i * Config::num_box_features + 8]; + float vel_x = boxes3d_vec[obj_i * Config::num_box_features + 9]; + float vel_y = boxes3d_vec[obj_i * Config::num_box_features + 10]; + + autoware_auto_perception_msgs::msg::DetectedObject obj; + // TODO(yukke42): the value of classification confidence of DNN, not probability. + obj.existence_probability = score; + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.probability = 1.0f; + classification.label = getSemanticType(class_names_[class_id]); + + 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; + } + + obj.classification.emplace_back(classification); + + obj.kinematics.pose_with_covariance.pose.position = autoware_utils::createPoint(x, y, z); + obj.kinematics.pose_with_covariance.pose.orientation = + autoware_utils::createQuaternionFromYaw(yaw); + obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions = autoware_utils::createTranslation(l, w, h); + + geometry_msgs::msg::Twist twist; + twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); + twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + obj.kinematics.twist_with_covariance.twist = twist; + obj.kinematics.has_twist = true; + + output_msg.objects.emplace_back(obj); + } + + if (objects_sub_count > 0) { + objects_pub_->publish(output_msg); + } + if (pointcloud_sub_count > 0) { + pointcloud_pub_->publish(stacked_pointcloud_msg); + } +} + +uint8_t LidarCenterPointNode::getSemanticType(const std::string & class_name) +{ + if (class_name == "CAR") { + return Label::CAR; + } else if (class_name == "TRUCK") { + return Label::TRUCK; + } else if (class_name == "BUS") { + return Label::BUS; + } else if (class_name == "TRAILER") { + return Label::TRAILER; + } else if (class_name == "BICYCLE") { + return Label::BICYCLE; + } else if (class_name == "MOTORBIKE") { + return Label::MOTORCYCLE; + } else if (class_name == "PEDESTRIAN") { + return Label::PEDESTRIAN; + } else { + return Label::UNKNOWN; + } +} + +bool LidarCenterPointNode::isCarLikeVehicleLabel(const uint8_t label) +{ + return label == Label::CAR || label == Label::TRUCK || label == Label::BUS || + label == Label::TRAILER; +} + +} // namespace centerpoint + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::LidarCenterPointNode)