feat: latest update

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

perception/image_projection_based_fusion/.gitignore

@@ -0,0 +1 @@
+/data/

perception/image_projection_based_fusion/CMakeLists.txt

@@ -7,6 +7,31 @@ find_package(OpenCV REQUIRED)
 find_package(Eigen3 REQUIRED)
 autoware_package()
 
+# Build non-CUDA dependent nodes
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/fusion_node.cpp
+  src/debugger.cpp
+  src/utils/geometry.cpp
+  src/utils/utils.cpp
+  src/roi_cluster_fusion/node.cpp
+  src/roi_detected_object_fusion/node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBRARIES}
+  ${EIGEN3_LIBRARIES}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode"
+  EXECUTABLE roi_detected_object_fusion_node
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "image_projection_based_fusion::RoiClusterFusionNode"
+  EXECUTABLE roi_cluster_fusion_node
+)
+
 set(CUDA_VERBOSE OFF)
 
 # set flags for CUDA availability
@@ -69,12 +94,15 @@ else()
   set(CUDNN_AVAIL OFF)
 endif()
 
-message(STATUS "start to download")
+# Create folder to store trained models.
+# NOTE: This must be created regardless of CUDA_AVAIL to be specified in ament_auto_package()
+set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data)
+execute_process(COMMAND mkdir -p ${DATA_PATH})
+
 if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
-# Download trained models
-  set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data)
-  execute_process(COMMAND mkdir -p ${DATA_PATH})
+  # Download trained models
 
+  message(STATUS "start to download")
   function(download FILE_NAME FILE_HASH)
     message(STATUS "Checking and downloading ${FILE_NAME}")
     set(FILE_PATH ${DATA_PATH}/${FILE_NAME})
@@ -89,15 +117,15 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
       else()
         message(STATUS "diff ${FILE_NAME}")
         message(STATUS "File hash changes. Downloading now ...")
-        file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v1/${FILE_NAME}
+        file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v2/${FILE_NAME}
                       ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
         list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
         list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
       endif()
     else()
       message(STATUS "not found ${FILE_NAME}")
       message(STATUS "File doesn't exists. Downloading now ...")
-      file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v1/${FILE_NAME}
+      file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v2/${FILE_NAME}
                     ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
       list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
       list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
@@ -110,8 +138,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   endfunction()
 
   # default model
-  download(pts_voxel_encoder_pointpainting.onnx 795a919537b71faae706b8c71312d31a)
-  download(pts_backbone_neck_head_pointpainting.onnx edb48a74659820904071ea8050413c23)
+  download(pts_voxel_encoder_pointpainting.onnx 438dfecd962631ec8f011e0dfa2c6160)
+  download(pts_backbone_neck_head_pointpainting.onnx e590a0b2bdcd35e01340cf4521bf149e)
   find_package(OpenCV REQUIRED)
   find_package(Eigen3 REQUIRED)
 
@@ -122,13 +150,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     ${PCL_INCLUDE_DIRS}
   )
 
-  ament_auto_add_library(${PROJECT_NAME} SHARED
-    src/fusion_node.cpp
-    src/debugger.cpp
-    src/utils/geometry.cpp
-    src/utils/utils.cpp
-    src/roi_cluster_fusion/node.cpp
-    src/roi_detected_object_fusion/node.cpp
+  ament_auto_add_library(pointpainting_lib SHARED
     src/pointpainting_fusion/node.cpp
     src/pointpainting_fusion/pointpainting_trt.cpp
     src/pointpainting_fusion/voxel_generator.cpp
@@ -138,7 +160,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     src/pointpainting_fusion/preprocess_kernel.cu
   )
 
-  target_link_libraries(${PROJECT_NAME}
+  target_link_libraries(pointpainting_lib
     ${OpenCV_LIBRARIES}
     ${EIGEN3_LIBRARIES}
     ${PCL_LIBRARIES}
@@ -150,30 +172,15 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     pointpainting_cuda_lib
   )
 
-  rclcpp_components_register_node(${PROJECT_NAME}
-    PLUGIN "image_projection_based_fusion::RoiClusterFusionNode"
-    EXECUTABLE roi_cluster_fusion_node
+  rclcpp_components_register_node(pointpainting_lib
+    PLUGIN "image_projection_based_fusion::PointpaintingFusionNode"
+    EXECUTABLE pointpainting_fusion_node
   )
-else()
-  find_package(ament_cmake_auto REQUIRED)
-  ament_auto_find_build_dependencies()
 
-  ament_auto_package(
-    INSTALL_TO_SHARE
-      launch
-  )
+else()
+  message("Skipping build of some nodes due to missing dependencies")
 endif()
 
-rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode"
-  EXECUTABLE roi_detected_object_fusion_node
-)
-
-rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "image_projection_based_fusion::PointpaintingFusionNode"
-  EXECUTABLE pointpainting_fusion_node
-)
-
 ament_auto_package(INSTALL_TO_SHARE
     launch
     config

perception/image_projection_based_fusion/config/pointpainting.param.yaml

@@ -2,9 +2,9 @@
   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
+    point_feature_size: 7  # x, y, z, timelag and car, pedestrian, bicycle
     max_voxel_size: 40000
     point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0]
     voxel_size: [0.32, 0.32, 8.0]
-    downsample_factor: 2
-    encoder_in_feature_size: 11
+    downsample_factor: 1
+    encoder_in_feature_size: 12

perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml

@@ -67,7 +67,7 @@
       <remap from="~/output/objects" to="$(var output/objects)"/>
       <param name="score_threshold" value="0.45"/>
       <param name="densification_world_frame_id" value="map"/>
-      <param name="densification_num_past_frames" value="0"/>
+      <param name="densification_num_past_frames" value="2"/>
       <param name="trt_precision" value="fp16"/>
       <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
       <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>

perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu

@@ -37,7 +37,7 @@ namespace
 {
 const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32;  // the same as max_point_in_voxel_size_ in config
 const std::size_t WARPS_PER_BLOCK = 4;
-const std::size_t ENCODER_IN_FEATURE_SIZE = 11;  // same as encoder_in_feature_size_ in config.hpp
+const std::size_t ENCODER_IN_FEATURE_SIZE = 12;  // same as encoder_in_feature_size_ in config.hpp
 
 std::size_t divup(const std::size_t a, const std::size_t b)
 {
@@ -70,7 +70,7 @@ __global__ void generateFeatures_kernel(
   if (pillar_idx >= num_voxels) return;
 
   // load src
-  const int POINT_FEATURE_SIZE = 6;
+  const int POINT_FEATURE_SIZE = 7;
   __shared__ float pillarSM[WARPS_PER_BLOCK][MAX_POINT_IN_VOXEL_SIZE][POINT_FEATURE_SIZE];
   __shared__ float3 pillarSumSM[WARPS_PER_BLOCK];
   __shared__ int3 cordsSM[WARPS_PER_BLOCK];
@@ -129,14 +129,15 @@ __global__ void generateFeatures_kernel(
     pillarOutSM[pillar_idx_inBlock][point_idx][3] = pillarSM[pillar_idx_inBlock][point_idx][3];
     pillarOutSM[pillar_idx_inBlock][point_idx][4] = pillarSM[pillar_idx_inBlock][point_idx][4];
     pillarOutSM[pillar_idx_inBlock][point_idx][5] = pillarSM[pillar_idx_inBlock][point_idx][5];
+    pillarOutSM[pillar_idx_inBlock][point_idx][6] = pillarSM[pillar_idx_inBlock][point_idx][6];
 
     // change index
-    pillarOutSM[pillar_idx_inBlock][point_idx][6] = mean.x;
-    pillarOutSM[pillar_idx_inBlock][point_idx][7] = mean.y;
-    pillarOutSM[pillar_idx_inBlock][point_idx][8] = mean.z;
+    pillarOutSM[pillar_idx_inBlock][point_idx][7] = mean.x;
+    pillarOutSM[pillar_idx_inBlock][point_idx][8] = mean.y;
+    pillarOutSM[pillar_idx_inBlock][point_idx][9] = mean.z;
 
-    pillarOutSM[pillar_idx_inBlock][point_idx][9] = center.x;
-    pillarOutSM[pillar_idx_inBlock][point_idx][10] = center.y;
+    pillarOutSM[pillar_idx_inBlock][point_idx][10] = center.x;
+    pillarOutSM[pillar_idx_inBlock][point_idx][11] = center.y;
 
   } else {
     pillarOutSM[pillar_idx_inBlock][point_idx][0] = 0;
@@ -152,6 +153,7 @@ __global__ void generateFeatures_kernel(
     pillarOutSM[pillar_idx_inBlock][point_idx][8] = 0;
     pillarOutSM[pillar_idx_inBlock][point_idx][9] = 0;
     pillarOutSM[pillar_idx_inBlock][point_idx][10] = 0;
+    pillarOutSM[pillar_idx_inBlock][point_idx][11] = 0;
   }
 
   __syncthreads();

perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp

@@ -44,19 +44,25 @@ std::size_t VoxelGenerator::pointsToVoxels(
   for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter);
        pc_cache_iter++) {
     auto pc_msg = pc_cache_iter->pointcloud_msg;
+    auto affine_past2current =
+      pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world;
+    float timelag = static_cast<float>(
+      pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds());
 
     for (sensor_msgs::PointCloud2ConstIterator<float> x_iter(pc_msg, "x"), y_iter(pc_msg, "y"),
          z_iter(pc_msg, "z"), car_iter(pc_msg, "CAR"), ped_iter(pc_msg, "PEDESTRIAN"),
          bic_iter(pc_msg, "BICYCLE");
          x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter, ++car_iter, ++ped_iter, ++bic_iter) {
-      point_current << *x_iter, *y_iter, *z_iter;
+      point_past << *x_iter, *y_iter, *z_iter;
+      point_current = affine_past2current * point_past;
 
       point[0] = point_current.x();
       point[1] = point_current.y();
       point[2] = point_current.z();
-      point[3] = *car_iter;
-      point[4] = *ped_iter;
-      point[5] = *bic_iter;
+      point[3] = timelag;
+      point[4] = *car_iter;
+      point[5] = *ped_iter;
+      point[6] = *bic_iter;
 
       out_of_range = false;
       for (std::size_t di = 0; di < config_.point_dim_size_; di++) {