Merge branch 'main' into fix/lidar_transfusion_param

Author: scepter914

.github/CODEOWNERS

@@ -6,7 +6,7 @@ common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.j
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai
 common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp
-common/autoware_point_types/** taichi.higashide@tier4.jp
+common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp
 common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp
 common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
@@ -115,9 +115,9 @@ perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4
 perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
 perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
-perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
-perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
+perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp
+perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp
 perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp
 perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp
 perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
@@ -192,10 +192,10 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_modu
 planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp
-planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp
+planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp
 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
-planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp
-planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp daniel.sanchez@tier4.jp alqudah.mohammad@tier4.jp
+planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp
+planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp
 planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp
@@ -204,7 +204,7 @@ sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minod
 sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp
 sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp
 sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp
+sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp
 sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -13,6 +13,7 @@
   <arg name="use_validator" description="use obstacle_pointcloud based validator"/>
   <arg name="objects_validation_method" description="options: `obstacle_pointcloud` or `occupancy_grid`"/>
   <arg name="use_low_intensity_cluster_filter"/>
+  <arg name="use_image_segmentation_based_filter"/>
   <arg name="use_multi_channel_tracker_merger"/>
 
   <!-- External interfaces -->
@@ -163,6 +164,7 @@
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
       <arg name="use_low_intensity_cluster_filter" value="$(var use_low_intensity_cluster_filter)"/>
+      <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
     </include>
   </group>
   <group if="$(var switch/detector/lidar_dnn)">

launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml

@@ -4,6 +4,7 @@
   <arg name="lidar_detection_model" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster"/>
   <arg name="use_low_intensity_cluster_filter"/>
+  <arg name="use_image_segmentation_based_filter"/>
 
   <!-- External interfaces -->
   <arg name="number_of_cameras"/>
@@ -95,14 +96,24 @@
     </include>
   </group>
 
+  <!-- Image_segmentation based filter, apply for camera0 only-->
+  <group>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/segmentation_pointcloud_fusion.launch.xml" if="$(var use_image_segmentation_based_filter)">
+      <arg name="input/pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+      <arg name="output/pointcloud" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud"/>
+    </include>
+  </group>
+
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
+      <let name="euclidean_cluster_input" value="$(var input/pointcloud_map/pointcloud)" unless="$(var use_image_segmentation_based_filter)"/>
+      <let name="euclidean_cluster_input" value="/perception/object_recognition/detection/segmentation_based_filtered/pointcloud" if="$(var use_image_segmentation_based_filter)"/>
       <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
       <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var input/pointcloud_map/pointcloud)"/>
+        <arg name="input_pointcloud" value="$(var euclidean_cluster_input)"/>
         <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -80,6 +80,7 @@
   <arg name="objects_filter_method" default="lanelet_filter" description="options: `lanelet_filter` or `position_filter`"/>
   <arg name="use_roi_based_cluster" default="true" description="use roi_based_cluster in clustering"/>
   <arg name="use_low_intensity_cluster_filter" default="true" description="use low_intensity_cluster_filter in clustering"/>
+  <arg name="use_image_segmentation_based_filter" default="false" description="use image_segmentation_based_filter in clustering"/>
   <arg
     name="use_empty_dynamic_object_publisher"
     default="false"
@@ -240,6 +241,7 @@
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
           <arg name="use_low_intensity_cluster_filter" value="$(var use_low_intensity_cluster_filter)"/>
+          <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
           <arg name="objects_filter_method" value="$(var objects_filter_method)"/>
           <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>

localization/ndt_scan_matcher/README.md

@@ -241,18 +241,26 @@ This is a function that uses no ground LiDAR scan to estimate the scan matching
 
 ### Abstract
 
-Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses.
-The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function.
+Initially, the covariance of NDT scan matching is a fixed value(FIXED_VALUE mode).
+So, three modes are provided for 2D covariance (xx, xy, yx, yy) estimation in real time: LAPLACE_APPROXIMATION, MULTI_NDT, and MULTI_NDT_SCORE.
+LAPLACE_APPROXIMATION calculates the inverse matrix of the XY (2x2) part of the Hessian obtained by NDT scan matching and uses it as the covariance matrix.
+On the other hand, MULTI_NDT, and MULTI_NDT_SCORE use NDT convergence from multiple initial poses to obtain 2D covariance.
+Ideally, the arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function.
 In this implementation, the number of initial positions is fixed to simplify the code.
+To obtain the covariance, MULTI_NDT computes until convergence at each initial position, while MULTI_NDT_SCORE uses the nearest voxel transformation likelihood.
 The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2.
 [original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/).
 
+<img src="./media/calculation_of_ndt_covariance.png" alt="drawing" width="600"/>
+
 Note that this function may spoil healthy system behavior if it consumes much calculation resources.
 
 ### Parameters
 
+There are three types in the calculation of 2D covariance in real time.You can select the method by changing covariance_estimation_type.
 initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix.
 initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
+In MULTI_NDT_SCORE mode, the scale of the output 2D covariance can be adjusted according to the temperature.
 
 {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }}
 

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -107,13 +107,18 @@
 
       # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
       covariance_estimation:
-        enable: false
+        # Covariance estimation type
+        # 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE
+        covariance_estimation_type: 0
 
         # Offset arrangement in covariance estimation [m]
         # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
         initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
         initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
 
+        # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance
+        temperature: 0.1
+
 
     dynamic_map_loading:
       # Dynamic map loading distance

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

@@ -29,6 +29,13 @@ enum class ConvergedParamType {
   NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
 };
 
+enum class CovarianceEstimationType {
+  FIXED_VALUE = 0,
+  LAPLACE_APPROXIMATION = 1,
+  MULTI_NDT = 2,
+  MULTI_NDT_SCORE = 3,
+};
+
 struct HyperParameters
 {
   struct Frame
@@ -80,8 +87,10 @@ struct HyperParameters
 
     struct CovarianceEstimation
     {
-      bool enable{};
-      std::vector<Eigen::Vector2d> initial_pose_offset_model{};
+      CovarianceEstimationType covariance_estimation_type{};
+      std::vector<double> initial_pose_offset_model_x{};
+      std::vector<double> initial_pose_offset_model_y{};
+      double temperature{};
     } covariance_estimation{};
   } covariance{};
 
@@ -148,33 +157,27 @@ struct HyperParameters
     for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
       covariance.output_pose_covariance[i] = output_pose_covariance[i];
     }
-    covariance.covariance_estimation.enable =
-      node->declare_parameter<bool>("covariance.covariance_estimation.enable");
-    if (covariance.covariance_estimation.enable) {
-      std::vector<double> initial_pose_offset_model_x =
-        node->declare_parameter<std::vector<double>>(
-          "covariance.covariance_estimation.initial_pose_offset_model_x");
-      std::vector<double> initial_pose_offset_model_y =
-        node->declare_parameter<std::vector<double>>(
-          "covariance.covariance_estimation.initial_pose_offset_model_y");
-
-      if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) {
-        const size_t size = initial_pose_offset_model_x.size();
-        covariance.covariance_estimation.initial_pose_offset_model.resize(size);
-        for (size_t i = 0; i < size; i++) {
-          covariance.covariance_estimation.initial_pose_offset_model[i].x() =
-            initial_pose_offset_model_x[i];
-          covariance.covariance_estimation.initial_pose_offset_model[i].y() =
-            initial_pose_offset_model_y[i];
-        }
-      } else {
-        std::stringstream message;
-        message << "Invalid initial pose offset model parameters."
-                << "Please make sure that the number of elements in "
-                << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same.";
-        throw std::runtime_error(message.str());
-      }
+    const int64_t covariance_estimation_type_tmp = node->declare_parameter<int64_t>(
+      "covariance.covariance_estimation.covariance_estimation_type");
+    covariance.covariance_estimation.covariance_estimation_type =
+      static_cast<CovarianceEstimationType>(covariance_estimation_type_tmp);
+    covariance.covariance_estimation.initial_pose_offset_model_x =
+      node->declare_parameter<std::vector<double>>(
+        "covariance.covariance_estimation.initial_pose_offset_model_x");
+    covariance.covariance_estimation.initial_pose_offset_model_y =
+      node->declare_parameter<std::vector<double>>(
+        "covariance.covariance_estimation.initial_pose_offset_model_y");
+    if (
+      covariance.covariance_estimation.initial_pose_offset_model_x.size() !=
+      covariance.covariance_estimation.initial_pose_offset_model_y.size()) {
+      std::stringstream message;
+      message << "Invalid initial pose offset model parameters."
+              << "Please make sure that the number of elements in "
+              << "initial_pose_offset_model_x and initial_pose_offset_model_y are the same.";
+      throw std::runtime_error(message.str());
     }
+    covariance.covariance_estimation.temperature =
+      node->declare_parameter<double>("covariance.covariance_estimation.temperature");
 
     dynamic_map_loading.update_distance =
       node->declare_parameter<double>("dynamic_map_loading.update_distance");

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -131,7 +131,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   static int count_oscillation(const std::vector<geometry_msgs::msg::Pose> & result_pose_msg_array);
 
-  std::array<double, 36> estimate_covariance(
+  Eigen::Matrix2d estimate_covariance(
     const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix,
     const rclcpp::Time & sensor_ros_time);
 

localization/ndt_scan_matcher/media/calculation_of_ndt_covariance.png

@@ -5,10 +5,12 @@
     "covariance_estimation": {
       "type": "object",
       "properties": {
-        "enable": {
-          "type": "boolean",
-          "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).",
-          "default": false
+        "covariance_estimation_type": {
+          "type": "number",
+          "description": "2D Real-time Converged estimation type. 0=FIXED_VALUE, 1=LAPLACE_APPROXIMATION, 2=MULTI_NDT, 3=MULTI_NDT_SCORE (FIXED_VALUE mode does not perform 2D Real-time Converged estimation)",
+          "default": 0,
+          "minimum": 0,
+          "maximum": 3
         },
         "initial_pose_offset_model_x": {
           "type": "array",
@@ -19,9 +21,20 @@
           "type": "array",
           "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.",
           "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
+        },
+        "temperature": {
+          "type": "number",
+          "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance",
+          "default": 0.1,
+          "exclusiveMinimum": 0
         }
       },
-      "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"],
+
+      "required": [
+        "covariance_estimation_type",
+        "initial_pose_offset_model_x",
+        "initial_pose_offset_model_y"
+      ],
       "additionalProperties": false
     }
   }