Skip to content

LiDAR Marker Localizer#

LiDARMarkerLocalizer is a detect-reflector-based localization node .

Inputs / Outputs#

lidar_marker_localizer node#

Input#

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

Output#

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#

Name Type Description Default Range
enable_read_all_target_ids boolean true N/A
target_ids array List of target marker IDs to be detected. ["0000", "0001", "0002", "0003", "0004", "0005"] N/A
queue_size_for_output_pose float Queue size for output/pose_with_covariance publisher 10 ≥0
marker_name string The name of the markers listed in the HD map. reflector N/A
road_surface_mode boolean Detect markers on the road surface instead of markers on a vertical surface. In this mode the lateral position is not observed: the y of the detected marker is fixed to marker_to_vehicle_offset_y, so the lateral position of the output pose is the mapped marker position minus this offset. Use this mode only where the lateral position of the vehicle relative to the marker is known (e.g. the vehicle always passes the marker on a fixed path). marker_height_from_ground is used as the height of the road surface marker (typically 0.0), reference_ring_number is not used, and the debug grids are drawn horizontally around marker_to_vehicle_offset_y. false N/A
resolution float Grid size for marker detection algorithm. [m] 0.05 ≥0.0
intensity_pattern array A sequence of high/low intensity to perform pattern matching. 1: high intensity (positive match), 0: not consider, -1: low intensity (negative match) [-1, -1, 0, 1, 1, 1, 1, 1, 0, -1, -1] N/A
match_intensity_difference_threshold float Threshold for determining high/low. 20 ≥0
positive_match_num_threshold float Threshold for the number of required matches with the pattern. 3 ≥0
negative_match_num_threshold float Threshold for the number of required matches with the pattern. 3 ≥0
vote_threshold_for_detect_marker float Threshold for the number of rings matching the pattern needed to detect it as a marker. 20 ≥0
marker_to_vehicle_offset_y float Y-axis offset from the center of the marker to vehicle. [m] 0.0 N/A
marker_height_from_ground float Height from the ground to the center of the marker. [m] 1.075 N/A
lower_ring_id_init float Initial value when searching for the lowest ring ID within the point cloud 128 ≥0
upper_ring_id_init float Initial value when searching for the highest ring ID within the point cloud 0 ≥0
reference_ring_number float Reference ring number when detecting the marker. Set 255 to disable ring filtering 255 N/A
self_pose_timeout_sec float Timeout for self pose. [sec] 1.0 ≥0.0
self_pose_distance_tolerance_m float Tolerance for the distance between two points when performing linear interpolation. [m] 1.0 ≥0.0
limit_distance_from_self_pose_to_nearest_marker float Distance limit for the purpose of determining whether the node should detect a marker. [m] 2.0 ≥0.0
limit_distance_from_self_pose_to_nearest_marker_y float Y-axis distance limit for the purpose of determining whether the node should detect a marker. [m] 1.0 ≥0.0
limit_distance_from_self_pose_to_marker float Distance limit for avoiding miss detection. [m] 2.0 ≥0.0
base_covariance array Output covariance in the base_link coordinate. [0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.569e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.569e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00030625] N/A
marker_width float Width of a marker. This param is used for visualizing the detected marker pointcloud[m] 0.8 ≥0.0
enable_save_log boolean false N/A
save_file_directory_path string $(env HOME)/detected_reflector_intensity N/A
save_file_name string detected_reflector_intensity N/A
save_frame_id string velodyne_top N/A
radius_for_extracting_marker_pointcloud float Only points closer than this value to the marker center on the xy plane are extracted as the “marker point cloud”. [m] 0.4 ≥0.0
queue_size_for_debug_pub_msg float Queue size for debug publishers 10 ≥0.0

How to launch#

When launching Autoware, set lidar-marker for pose_source.

ros2 launch autoware_launch ... \
    pose_source:=lidar-marker \
    ...

Design#

Flowchart#

uml diagram

Detection Algorithm#

detection_algorithm

  1. Split the LiDAR point cloud into rings along the x-axis of the base_link coordinate system at intervals of the resolution size.
  2. Find the portion of intensity that matches the intensity_pattern.
  3. Perform steps 1 and 2 for each ring, accumulate the matching indices, and detect portions where the count exceeds the vote_threshold_for_detect_marker as markers.

Sample Dataset#

This dataset was acquired in National Institute for Land and Infrastructure Management, Full-scale tunnel experiment facility. The reflectors were installed by Taisei Corporation.

Collaborators#

  1. Assumed ring of PointXYZIRADRT as channel of PointXYZIRC. ↩