feat(lidar_marker_localizer): road surface lidar marker extension

localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp

@@ -43,6 +43,9 @@ class LandmarkManager
   void parse_landmarks(
     const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg,
     const std::string & target_subtype);
+  void parse_landmarks(
+    const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg,
+    const std::string & target_subtype, const std::vector<std::string> target_ids);
 
   [[nodiscard]] std::vector<landmark_manager::Landmark> get_landmarks() const;
 

localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp

@@ -53,12 +53,63 @@
     const auto & v1 = vertices[1];
     const auto & v2 = vertices[2];
     const auto & v3 = vertices[3];
-    const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0;
-    const double volume_threshold = 1e-3;
-    if (volume > volume_threshold) {
+
+    // Calculate the center of the quadrilateral
+    const auto center = (v0 + v1 + v2 + v3) / 4.0;
+
+    // Define axes
+    const auto x_axis = (v1 - v0).normalized();
+    const auto y_axis = (v2 - v1).normalized();
+    const auto z_axis = x_axis.cross(y_axis).normalized();
+
+    // Construct rotation matrix and convert it to quaternion
+    Eigen::Matrix3d rotation_matrix;
+    rotation_matrix << x_axis, y_axis, z_axis;
+    const Eigen::Quaterniond q{rotation_matrix};
+
+    // Create Pose
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = center.x();
+    pose.position.y = center.y();
+    pose.position.z = center.z();
+    pose.orientation.x = q.x();
+    pose.orientation.y = q.y();
+    pose.orientation.z = q.z();
+    pose.orientation.w = q.w();
+
+    // Add
+    landmarks_map_[landmark_id].push_back(pose);
+  }
+}
+
+void LandmarkManager::parse_landmarks(
+  const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg,
+  const std::string & target_subtype, const std::vector<std::string> target_ids)
+{
+  std::vector<lanelet::Polygon3d> landmarks =
+    lanelet::localization::parseLandmarkPolygons(msg, target_subtype);
+  for (const lanelet::Polygon3d & poly : landmarks) {
+    // Get landmark_id
+    const std::string landmark_id = poly.attributeOr("marker_id", "none");
+
+    auto find_it = std::find(std::begin(target_ids), std::end(target_ids), landmark_id);
+    if (find_it == std::end(target_ids)) {
+      continue;
+    }
+
+    // Get 4 vertices
+    const auto & vertices = poly.basicPolygon();
+    if (vertices.size() != 4) {
       continue;
     }
 
+    // 4 vertices represent the landmark vertices in counterclockwise order
+    // Calculate the volume by considering it as a tetrahedron
+    const auto & v0 = vertices[0];
+    const auto & v1 = vertices[1];
+    const auto & v2 = vertices[2];
+    const auto & v3 = vertices[3];
+
     // Calculate the center of the quadrilateral
     const auto center = (v0 + v1 + v2 + v3) / 4.0;
 
@@ -166,49 +217,47 @@
     // convert base_link to map
     const Pose detected_landmark_on_map =
       autoware_utils_geometry::transform_pose(detected_landmark_on_base_link, self_pose);
 
-    // match to map
-    if (landmarks_map_.count(landmark.id) == 0) {
-      continue;
-    }
-
     // check all poses
-    for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) {
-      // check distance
-      const double curr_distance = autoware_utils_geometry::calc_distance3d(
-        mapped_landmark_on_map.position, detected_landmark_on_map.position);
-      if (curr_distance > min_distance) {
-        continue;
-      }
-
-      if (consider_orientation) {
-        const Eigen::Affine3d landmark_pose =
-          autoware::localization_util::pose_to_affine3d(mapped_landmark_on_map);
-        const Eigen::Affine3d landmark_to_base_link =
-          autoware::localization_util::pose_to_affine3d(detected_landmark_on_base_link).inverse();
-        const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link;
-
-        const Pose new_self_pose =
-          autoware::localization_util::matrix4f_to_pose(new_self_pose_eigen.matrix().cast<float>());
-
-        // update
-        min_distance = curr_distance;
-        min_new_self_pose = new_self_pose;
-      } else {
-        const double diff_x =
-          mapped_landmark_on_map.position.x - detected_landmark_on_map.position.x;
-        const double diff_y =
-          mapped_landmark_on_map.position.y - detected_landmark_on_map.position.y;
-        const double diff_z =
-          mapped_landmark_on_map.position.z - detected_landmark_on_map.position.z;
-        Pose new_self_pose = self_pose;
-        new_self_pose.position.x += diff_x;
-        new_self_pose.position.y += diff_y;
-        new_self_pose.position.z += diff_z;
-
-        // update
-        min_distance = curr_distance;
-        min_new_self_pose = new_self_pose;
+    // for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) {
+    for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) {
+      for (const auto & mapped_landmark_on_map : landmark_poses) {
+        // check distance
+        const double curr_distance = autoware_utils_geometry::calc_distance3d(
+          mapped_landmark_on_map.position, detected_landmark_on_map.position);
+        if (curr_distance > min_distance) {
+          continue;
+        }
+
+        if (consider_orientation) {
+          const Eigen::Affine3d landmark_pose =
+            autoware::localization_util::pose_to_affine3d(mapped_landmark_on_map);
+          const Eigen::Affine3d landmark_to_base_link =
+            autoware::localization_util::pose_to_affine3d(detected_landmark_on_base_link).inverse();
+          const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link;
+
+          const Pose new_self_pose = autoware::localization_util::matrix4f_to_pose(
+            new_self_pose_eigen.matrix().cast<float>());
+
+          // update
+          min_distance = curr_distance;
+          min_new_self_pose = new_self_pose;
+        } else {
+          const double diff_x =
+            mapped_landmark_on_map.position.x - detected_landmark_on_map.position.x;
+          const double diff_y =
+            mapped_landmark_on_map.position.y - detected_landmark_on_map.position.y;
+          const double diff_z =
+            mapped_landmark_on_map.position.z - detected_landmark_on_map.position.z;
+          Pose new_self_pose = self_pose;
+          new_self_pose.position.x += diff_x;
+          new_self_pose.position.y += diff_y;
+          new_self_pose.position.z += diff_z;
+
+          // update
+          min_distance = curr_distance;
+          min_new_self_pose = new_self_pose;
+        }
       }
     }
   }

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt

@@ -27,6 +27,14 @@ rclcpp_components_register_node(${PROJECT_NAME}
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_lidar_marker_localizer test/test_lidar_marker_localizer.cpp)
+  target_include_directories(test_lidar_marker_localizer PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
+  target_link_libraries(test_lidar_marker_localizer
+    ${PROJECT_NAME}
+    ${PCL_LIBRARIES}
+  )
+  ament_target_dependencies(test_lidar_marker_localizer rclcpp rclcpp_components)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/README.md

@@ -8,22 +8,34 @@
 
 #### Input
 
-| Name                   | Type                                            | Description      |
-| :--------------------- | :---------------------------------------------- | :--------------- |
-| `~/input/lanelet2_map` | `autoware_map_msgs::msg::HADMapBin`             | Data of lanelet2 |
-| `~/input/pointcloud`   | `sensor_msgs::msg::PointCloud2`                 | PointCloud       |
-| `~/input/ekf_pose`     | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose         |
+| Name                   | Type                                            | Description                                                                                                    |
+| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------- |
+| `~/input/lanelet2_map` | `autoware_map_msgs::msg::HADMapBin`             | Data of lanelet2                                                                                               |
+| `~/input/pointcloud`   | `sensor_msgs::msg::PointCloud2`                 | PointType: `PointXYZIRC` is recommended in Autoware, but `PointXYZIRADRT` are also supported in this node.[^1] |
+| `~/input/ekf_pose`     | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose                                                                                                       |
+
+[^1]: Assumed `ring` of `PointXYZIRADRT` as `channel` of `PointXYZIRC`.
 
 #### Output
 
-| Name                            | Type                                            | Description                                                        |
-| :------------------------------ | :---------------------------------------------- | :----------------------------------------------------------------- |
-| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose                                                     |
-| `~/debug/pose_with_covariance`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] Estimated pose                                       |
-| `~/debug/marker_detected`       | `geometry_msgs::msg::PoseArray`                 | [debug topic] Detected marker poses                                |
-| `~/debug/marker_mapped`         | `visualization_msgs::msg::MarkerArray`          | [debug topic] Loaded landmarks to visualize in Rviz as thin boards |
-| `~/debug/marker_pointcloud`     | `sensor_msgs::msg::PointCloud2`                 | [debug topic] PointCloud of the detected marker                    |
-| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs                                                |
+| Name                            | Type                                            | Description         |
+| :------------------------------ | :---------------------------------------------- | :------------------ |
+| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose      |
+| `/diagnostics`                  | `diagnostic_msgs::msg::DiagnosticArray`         | Diagnostics outputs |
+
+##### For debug
+
+| Name                            | Type                                            | Description                                          |
+| :------------------------------ | :---------------------------------------------- | :--------------------------------------------------- |
+| `~/debug/pose_with_covariance`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose                                       |
+| `~/debug/marker_detected`       | `geometry_msgs::msg::PoseArray`                 | Detected marker poses                                |
+| `~/debug/marker_mapped`         | `visualization_msgs::msg::MarkerArray`          | Loaded landmarks to visualize in Rviz as thin boards |
+| `~/debug/marker_pointcloud`     | `sensor_msgs::msg::PointCloud2`                 | PointCloud of the detected marker                    |
+| `~/debug/center_intensity_grid` | `nav_msgs::msg::OccupancyGrid`                  | Center intensity grid for debug                      |
+| `~/debug/positive_grid`         | `nav_msgs::msg::OccupancyGrid`                  | Positive match grid for debug                        |
+| `~/debug/negative_grid`         | `nav_msgs::msg::OccupancyGrid`                  | Negative match grid for debug                        |
+| `~/debug/matched_grid`          | `nav_msgs::msg::OccupancyGrid`                  | Matched pattern grid for debug                       |
+| `~/debug/vote_grid`             | `nav_msgs::msg::OccupancyGrid`                  | Vote grid for marker detection debug                 |
 
 ## Parameters
 
@@ -111,4 +123,4 @@ The reflectors were installed by [Taisei Corporation](https://www.taisei.co.jp/e
 
 - [TIER IV](https://tier4.jp/en/)
 - [Taisei Corporation](https://www.taisei.co.jp/english/)
-- [Yuri Shimizu](https://github.com/YuriShimizu824)
+  - [Yuri Shimizu](https://github.com/YuriShimizu824)

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/config/lidar_marker_localizer.param.yaml

@@ -1,10 +1,14 @@
 /**:
   ros__parameters:
+    enable_read_all_target_ids: true
+    target_ids: ["0000", "0001", "0002", "0003", "0004", "0005"]
+    queue_size_for_output_pose: 10 # Queue size for output/pose_with_covariance publisher
 
     # marker name
     marker_name: "reflector"
 
     # for marker detection algorithm
+    road_surface_mode: false
     resolution: 0.05
     # A sequence of high/low intensity to perform pattern matching.
     # 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match)
@@ -13,14 +17,19 @@
     positive_match_num_threshold: 3
     negative_match_num_threshold: 3
     vote_threshold_for_detect_marker: 20
+    marker_to_vehicle_offset_y: 0.0
     marker_height_from_ground: 1.075
+    lower_ring_id_init: 128
+    upper_ring_id_init: 0
+    reference_ring_number: 255 # set to 255 to disable ring filtering
 
     # for interpolate algorithm
     self_pose_timeout_sec: 1.0
     self_pose_distance_tolerance_m: 1.0
 
     # for validation
     limit_distance_from_self_pose_to_nearest_marker: 2.0
+    limit_distance_from_self_pose_to_nearest_marker_y: 1.0
     limit_distance_from_self_pose_to_marker: 2.0
 
     # base_covariance
@@ -40,3 +49,5 @@
     save_file_directory_path: detected_reflector_intensity
     save_file_name: detected_reflector_intensity
     save_frame_id: velodyne_top
+    radius_for_extracting_marker_pointcloud: 0.4 # [m] within
+    queue_size_for_debug_pub_msg: 10 # Queue size for debug publishers

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml

@@ -17,9 +17,9 @@
   <depend>autoware_localization_util</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_point_types</depend>
+  <depend>autoware_pointcloud_preprocessor</depend>
   <depend>autoware_qos_utils</depend>
-  <depend>autoware_utils_diagnostics</depend>
-  <depend>autoware_utils_geometry</depend>
+  <depend>autoware_utils</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>

localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/schema/lidar_marker_localizer.schema.json

@@ -6,11 +6,32 @@
     "lidar_marker_localizer": {
       "type": "object",
       "properties": {
+        "enable_read_all_target_ids": {
+          "type": "boolean",
+          "description": "",
+          "default": true
+        },
+        "target_ids": {
+          "type": "array",
+          "description": "List of target marker IDs to be detected.",
+          "default": ["0000", "0001", "0002", "0003", "0004", "0005"]
+        },
+        "queue_size_for_output_pose": {
+          "type": "number",
+          "description": "Queue size for output/pose_with_covariance publisher",
+          "default": 10,
+          "minimum": 0
+        },
         "marker_name": {
           "type": "string",
           "description": "The name of the markers listed in the HD map.",
           "default": "reflector"
         },
+        "road_surface_mode": {
+          "type": "boolean",
+          "description": "Enable road surface mode for marker detection.",
+          "default": false
+        },
         "resolution": {
           "type": "number",
           "description": "Grid size for marker detection algorithm. [m]",
@@ -46,11 +67,33 @@
           "default": 20,
           "minimum": 0
         },
+        "marker_to_vehicle_offset_y": {
+          "type": "number",
+          "description": "Y-axis offset from the center of the marker to vehicle. [m]",
+          "default": 0.0
+        },
         "marker_height_from_ground": {
           "type": "number",
           "description": "Height from the ground to the center of the marker. [m]",
           "default": 1.075
         },
+        "lower_ring_id_init": {
+          "type": "number",
+          "description": "Initial value when searching for the lowest ring ID within the point cloud",
+          "default": 128,
+          "minimum": 0
+        },
+        "upper_ring_id_init": {
+          "type": "number",
+          "description": "Initial value when searching for the highest ring ID within the point cloud",
+          "default": 0,
+          "minimum": 0
+        },
+        "reference_ring_number": {
+          "type": "number",
+          "description": "Reference ring number when detecting the marker. Set 255 to disable ring filtering",
+          "default": 255
+        },
         "self_pose_timeout_sec": {
           "type": "number",
           "description": "Timeout for self pose. [sec]",
@@ -69,6 +112,12 @@
           "default": 2.0,
           "minimum": 0.0
         },
+        "limit_distance_from_self_pose_to_nearest_marker_y": {
+          "type": "number",
+          "description": "Y-axis distance limit for the purpose of determining whether the node should detect a marker. [m]",
+          "default": 1.0,
+          "minimum": 0.0
+        },
         "limit_distance_from_self_pose_to_marker": {
           "type": "number",
           "description": "Distance limit for avoiding miss detection. [m]",
@@ -109,6 +158,18 @@
           "type": "string",
           "description": "",
           "default": "velodyne_top"
+        },
+        "radius_for_extracting_marker_pointcloud": {
+          "type": "number",
+          "description": "Only points closer than this value to the marker center on the xy plane are extracted as the “marker point cloud”. [m]",
+          "default": 0.4,
+          "minimum": 0.0
+        },
+        "queue_size_for_debug_pub_msg": {
+          "type": "number",
+          "description": "Queue size for debug publishers",
+          "default": 10,
+          "minimum": 0.0
         }
       },
       "required": [
@@ -118,9 +179,13 @@
         "positive_match_num_threshold",
         "negative_match_num_threshold",
         "vote_threshold_for_detect_marker",
+        "marker_to_vehicle_offset_y",
+        "marker_height_from_ground",
+        "reference_ring_number",
         "self_pose_timeout_sec",
         "self_pose_distance_tolerance_m",
         "limit_distance_from_self_pose_to_nearest_marker",
+        "limit_distance_from_self_pose_to_nearest_marker_y",
         "limit_distance_from_self_pose_to_marker",
         "base_covariance",
         "marker_width",