feat(lidar_centerpoint): make lidar_centerpoint a shared library (tie…

…r4#733) * refactor: sererate config.hpp Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: set config from ros params Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: change directory structure Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: make centerpoint shared library Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: to detected objects message Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: change var name Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * refactor: config Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: remove src directory from target_include_directories Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
boyali · Oct 19, 2022 · 89569c7 · 89569c7
1 parent b933697
commit 89569c7
Show file tree

Hide file tree

Showing 30 changed files with 576 additions and 377 deletions.
diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt
@@ -110,40 +110,47 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   ament_auto_find_build_dependencies()
 
   include_directories(
-    lib/include
+    include
     ${CUDA_INCLUDE_DIRS}
   )
 
   ### centerpoint ###
-  ament_auto_add_library(centerpoint SHARED
-    lib/src/centerpoint_trt.cpp
-    lib/src/pointcloud_densification.cpp
-    lib/src/voxel_generator.cpp
-    lib/src/tensorrt_wrapper.cpp
-    lib/src/network_trt.cpp
-    lib/src/utils.cpp
+  ament_auto_add_library(centerpoint_lib SHARED
+    lib/centerpoint_trt.cpp
+    lib/utils.cpp
+    lib/ros_utils.cpp
+    lib/network/network_trt.cpp
+    lib/network/tensorrt_wrapper.cpp
+    lib/preprocess/pointcloud_densification.cpp
+    lib/preprocess/voxel_generator.cpp
   )
 
-  cuda_add_library(centerpoint_cuda_libraries SHARED
-    lib/src/circle_nms_kernel.cu
-    lib/src/postprocess_kernel.cu
-    lib/src/preprocess_kernel.cu
-    lib/src/scatter_kernel.cu
+  cuda_add_library(centerpoint_cuda_lib SHARED
+    lib/postprocess/circle_nms_kernel.cu
+    lib/postprocess/postprocess_kernel.cu
+    lib/network/scatter_kernel.cu
+    lib/preprocess/preprocess_kernel.cu
   )
 
-  target_link_libraries(centerpoint
+  target_link_libraries(centerpoint_lib
     ${NVINFER}
     ${NVONNXPARSER}
     ${CUDA_LIBRARIES}
     ${CUBLAS_LIBRARIES}
     ${CUDA_curand_LIBRARY}
     ${CUDNN_LIBRARY}
-    centerpoint_cuda_libraries
+    centerpoint_cuda_lib
+  )
+
+  target_include_directories(centerpoint_lib
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
   )
 
   # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6.
   # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe
-  target_include_directories(centerpoint
+  target_include_directories(centerpoint_lib
     SYSTEM PUBLIC
       ${CUDA_INCLUDE_DIRS}
   )
@@ -154,7 +161,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   )
 
   target_link_libraries(lidar_centerpoint_component
-    centerpoint
+    centerpoint_lib
   )
 
   rclcpp_components_register_node(lidar_centerpoint_component

diff --git a/perception/lidar_centerpoint/config/aip_x2.param.yaml b/perception/lidar_centerpoint/config/aip_x2.param.yaml
@@ -1,3 +1,10 @@
 /**:
   ros__parameters:
     class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
+    rename_car_to_truck_and_bus: false
+    point_feature_size: 4
+    max_voxel_size: 40000
+    point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0]
+    voxel_size: [0.32, 0.32, 8.0]
+    downsample_factor: 2
+    encoder_in_feature_size: 9
diff --git a/perception/lidar_centerpoint/config/default.param.yaml b/perception/lidar_centerpoint/config/default.param.yaml
@@ -2,3 +2,9 @@
   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]
+    voxel_size: [0.32, 0.32, 8.0]
+    downsample_factor: 2
+    encoder_in_feature_size: 9
diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp
@@ -0,0 +1,114 @@
+// 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__CENTERPOINT_CONFIG_HPP_
+#define LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_
+
+#include <cstddef>
+#include <vector>
+
+namespace centerpoint
+{
+class CenterPointConfig
+{
+public:
+  explicit CenterPointConfig(
+    const std::size_t class_size, const float point_feature_size, const std::size_t max_voxel_size,
+    const std::vector<double> & point_cloud_range, const std::vector<double> & voxel_size,
+    const std::size_t downsample_factor, const std::size_t encoder_in_feature_size,
+    const float score_threshold, const float circle_nms_dist_threshold)
+  {
+    class_size_ = class_size;
+    point_feature_size_ = point_feature_size;
+    max_voxel_size_ = max_voxel_size;
+    if (point_cloud_range.size() == 6) {
+      range_min_x_ = static_cast<float>(point_cloud_range[0]);
+      range_min_y_ = static_cast<float>(point_cloud_range[1]);
+      range_min_z_ = static_cast<float>(point_cloud_range[2]);
+      range_max_x_ = static_cast<float>(point_cloud_range[3]);
+      range_max_y_ = static_cast<float>(point_cloud_range[4]);
+      range_max_z_ = static_cast<float>(point_cloud_range[5]);
+    }
+    if (voxel_size.size() == 3) {
+      voxel_size_x_ = static_cast<float>(voxel_size[0]);
+      voxel_size_y_ = static_cast<float>(voxel_size[1]);
+      voxel_size_z_ = static_cast<float>(voxel_size[2]);
+    }
+    downsample_factor_ = downsample_factor;
+    encoder_in_feature_size_ = encoder_in_feature_size;
+
+    if (score_threshold > 0 && score_threshold < 1) {
+      score_threshold_ = score_threshold;
+    }
+
+    if (circle_nms_dist_threshold > 0) {
+      circle_nms_dist_threshold_ = circle_nms_dist_threshold;
+    }
+
+    grid_size_x_ = static_cast<std::size_t>((range_max_x_ - range_min_x_) / voxel_size_x_);
+    grid_size_y_ = static_cast<std::size_t>((range_max_y_ - range_min_y_) / voxel_size_y_);
+    grid_size_z_ = static_cast<std::size_t>((range_max_z_ - range_min_z_) / voxel_size_z_);
+    offset_x_ = range_min_x_ + voxel_size_x_ / 2;
+    offset_y_ = range_min_y_ + voxel_size_y_ / 2;
+    offset_z_ = range_min_z_ + voxel_size_z_ / 2;
+    down_grid_size_x_ = grid_size_x_ / downsample_factor_;
+    down_grid_size_y_ = grid_size_y_ / downsample_factor_;
+  };
+
+  // input params
+  std::size_t class_size_{3};
+  const std::size_t point_dim_size_{3};  // x, y and z
+  std::size_t point_feature_size_{4};    // x, y, z and timelag
+  std::size_t max_point_in_voxel_size_{32};
+  std::size_t max_voxel_size_{40000};
+  float range_min_x_{-89.6f};
+  float range_min_y_{-89.6f};
+  float range_min_z_{-3.0f};
+  float range_max_x_{89.6f};
+  float range_max_y_{89.6f};
+  float range_max_z_{5.0f};
+  float voxel_size_x_{0.32f};
+  float voxel_size_y_{0.32f};
+  float voxel_size_z_{8.0f};
+
+  // network params
+  const std::size_t batch_size_{1};
+  std::size_t downsample_factor_{2};
+  std::size_t encoder_in_feature_size_{9};
+  const std::size_t encoder_out_feature_size_{32};
+  const std::size_t head_out_size_{6};
+  const std::size_t head_out_offset_size_{2};
+  const std::size_t head_out_z_size_{1};
+  const std::size_t head_out_dim_size_{3};
+  const std::size_t head_out_rot_size_{2};
+  const std::size_t head_out_vel_size_{2};
+
+  // post-process params
+  float score_threshold_{0.4f};
+  float circle_nms_dist_threshold_{1.5f};
+
+  // calculated params
+  std::size_t grid_size_x_ = (range_max_x_ - range_min_x_) / voxel_size_x_;
+  std::size_t grid_size_y_ = (range_max_y_ - range_min_y_) / voxel_size_y_;
+  std::size_t grid_size_z_ = (range_max_z_ - range_min_z_) / voxel_size_z_;
+  float offset_x_ = range_min_x_ + voxel_size_x_ / 2;
+  float offset_y_ = range_min_y_ + voxel_size_y_ / 2;
+  float offset_z_ = range_min_z_ + voxel_size_z_ / 2;
+  std::size_t down_grid_size_x_ = grid_size_x_ / downsample_factor_;
+  std::size_t down_grid_size_y_ = grid_size_y_ / downsample_factor_;
+};
+
+}  // namespace centerpoint
+
+#endif  // LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_
diff --git a/...nterpoint/lib/include/centerpoint_trt.hpp → ...ude/lidar_centerpoint/centerpoint_trt.hpp b/...nterpoint/lib/include/centerpoint_trt.hpp → ...ude/lidar_centerpoint/centerpoint_trt.hpp
@@ -12,14 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef CENTERPOINT_TRT_HPP_
-#define CENTERPOINT_TRT_HPP_
+#ifndef LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_
+#define LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_
 
-#include <config.hpp>
-#include <cuda_utils.hpp>
-#include <network_trt.hpp>
-#include <postprocess_kernel.hpp>
-#include <voxel_generator.hpp>
+#include <lidar_centerpoint/cuda_utils.hpp>
+#include <lidar_centerpoint/network/network_trt.hpp>
+#include <lidar_centerpoint/postprocess/postprocess_kernel.hpp>
+#include <lidar_centerpoint/preprocess/voxel_generator.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
@@ -57,8 +56,8 @@ class CenterPointTRT
 {
 public:
   explicit CenterPointTRT(
-    const std::size_t num_class, const float score_threshold, const NetworkParam & encoder_param,
-    const NetworkParam & head_param, const DensificationParam & densification_param);
+    const NetworkParam & encoder_param, const NetworkParam & head_param,
+    const DensificationParam & densification_param, const CenterPointConfig & config);
 
   ~CenterPointTRT();
 
@@ -83,7 +82,8 @@ class CenterPointTRT
   cudaStream_t stream_{nullptr};
 
   bool verbose_{false};
-  std::size_t num_class_{0};
+  std::size_t class_size_{0};
+  CenterPointConfig config_;
   std::size_t num_voxels_{0};
   std::size_t encoder_in_feature_size_{0};
   std::size_t spatial_features_size_{0};
@@ -106,4 +106,4 @@ class CenterPointTRT
 
 }  // namespace centerpoint
 
-#endif  // CENTERPOINT_TRT_HPP_
+#endif  // LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_
diff --git a/...ar_centerpoint/lib/include/cuda_utils.hpp → .../include/lidar_centerpoint/cuda_utils.hpp b/...ar_centerpoint/lib/include/cuda_utils.hpp → .../include/lidar_centerpoint/cuda_utils.hpp
@@ -39,8 +39,8 @@
  * https://creativecommons.org/publicdomain/zero/1.0/deed.en
  */
 
-#ifndef CUDA_UTILS_HPP_
-#define CUDA_UTILS_HPP_
+#ifndef LIDAR_CENTERPOINT__CUDA_UTILS_HPP_
+#define LIDAR_CENTERPOINT__CUDA_UTILS_HPP_
 
 #include <cuda_runtime_api.h>
 
@@ -117,4 +117,4 @@ inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_s
 
 }  // namespace cuda
 
-#endif  // CUDA_UTILS_HPP_
+#endif  // LIDAR_CENTERPOINT__CUDA_UTILS_HPP_
diff --git a/...r_centerpoint/lib/include/network_trt.hpp → ...lidar_centerpoint/network/network_trt.hpp b/...r_centerpoint/lib/include/network_trt.hpp → ...lidar_centerpoint/network/network_trt.hpp
@@ -12,10 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NETWORK_TRT_HPP_
-#define NETWORK_TRT_HPP_
+#ifndef LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_
+#define LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_
 
-#include <tensorrt_wrapper.hpp>
+#include <lidar_centerpoint/centerpoint_config.hpp>
+#include <lidar_centerpoint/network/tensorrt_wrapper.hpp>
+
+#include <vector>
 
 namespace centerpoint
 {
@@ -35,16 +38,18 @@ class HeadTRT : public TensorRTWrapper
 public:
   using TensorRTWrapper::TensorRTWrapper;
 
-  HeadTRT(const std::size_t num_class, const bool verbose);
+  HeadTRT(
+    const std::vector<std::size_t> & out_channel_sizes, const CenterPointConfig & config,
+    const bool verbose);
 
 protected:
   bool setProfile(
     nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network,
     nvinfer1::IBuilderConfig & config) override;
 
-  std::size_t num_class_{0};
+  std::vector<std::size_t> out_channel_sizes_;
 };
 
 }  // namespace centerpoint
 
-#endif  // NETWORK_TRT_HPP_
+#endif  // LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_
diff --git a/...enterpoint/lib/include/scatter_kernel.hpp → ...ar_centerpoint/network/scatter_kernel.hpp b/...enterpoint/lib/include/scatter_kernel.hpp → ...ar_centerpoint/network/scatter_kernel.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCATTER_KERNEL_HPP_
-#define SCATTER_KERNEL_HPP_
+#ifndef LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_
+#define LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_
 
 #include <cuda.h>
 #include <cuda_runtime_api.h>
@@ -22,8 +22,10 @@ namespace centerpoint
 {
 cudaError_t scatterFeatures_launch(
   const float * pillar_features, const int * coords, const std::size_t num_pillars,
-  float * scattered_features, cudaStream_t stream);
+  const std::size_t max_voxel_size, const std::size_t encoder_out_feature_size,
+  const std::size_t grid_size_x, const std::size_t grid_size_y, float * scattered_features,
+  cudaStream_t stream);
 
 }  // namespace centerpoint
 
-#endif  // SCATTER_KERNEL_HPP_
+#endif  // LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_
diff --git a/...terpoint/lib/include/tensorrt_wrapper.hpp → ..._centerpoint/network/tensorrt_wrapper.hpp b/...terpoint/lib/include/tensorrt_wrapper.hpp → ..._centerpoint/network/tensorrt_wrapper.hpp
@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TENSORRT_WRAPPER_HPP_
-#define TENSORRT_WRAPPER_HPP_
+#ifndef LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_
+#define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_
+
+#include <lidar_centerpoint/centerpoint_config.hpp>
 
 #include <NvInfer.h>
 
@@ -56,7 +58,7 @@ class Logger : public nvinfer1::ILogger
 class TensorRTWrapper
 {
 public:
-  explicit TensorRTWrapper(bool verbose);
+  explicit TensorRTWrapper(const CenterPointConfig & config, const bool verbose);
 
   bool init(
     const std::string & onnx_path, const std::string & engine_path, const std::string & precision);
@@ -68,6 +70,7 @@ class TensorRTWrapper
     nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network,
     nvinfer1::IBuilderConfig & config) = 0;
 
+  CenterPointConfig config_;
   Logger logger_;
 
 private:
@@ -88,4 +91,4 @@ class TensorRTWrapper
 
 }  // namespace centerpoint
 
-#endif  // TENSORRT_WRAPPER_HPP_
+#endif  // LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_