diff --git a/docs/how-to-guides/index.md b/docs/how-to-guides/index.md index 8d5aabb0274..2686b037856 100644 --- a/docs/how-to-guides/index.md +++ b/docs/how-to-guides/index.md @@ -14,6 +14,7 @@ - [Advanced usage of colcon](others/advanced-usage-of-colcon.md) - [Applying Clang-Tidy to ROS packages](others/applying-clang-tidy-to-ros-packages.md) - [Using Eagleye in Autoware](others/eagleye-integration-guide.md) +- [Using YabLoc in Autoware](others/yabloc-guide.md) TODO: Write the following contents. diff --git a/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_architecture.drawio.svg b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_architecture.drawio.svg new file mode 100644 index 00000000000..e4937886344 --- /dev/null +++ b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_architecture.drawio.svg @@ -0,0 +1,739 @@ + + + + + + + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22camera_pose_%26lt%3Bbr%26gt%3Binitializer%22%20style%3D%22rounded%3D0%3BwhiteSpace%3Dwrap%3Bhtml%3D1%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22360%22%20y%3D%22200%22%20width%3D%22120%22%20height%3D%2240%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + +
+
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%... + +
+
+ + + + + + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22camera_pose_%26lt%3Bbr%26gt%3Binitializer%22%20style%3D%22rounded%3D0%3BwhiteSpace%3Dwrap%3Bhtml%3D1%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22360%22%20y%3D%22200%22%20width%3D%22120%22%20height%3D%2240%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + +
+
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%... + +
+
+ + + + + + +
+
+
+ Camera +
+
+
+
+ + Camera + +
+
+ + + + + + + +
+
+
+ /map/vectormap +
+
+
+
+ + /map/vectormap + +
+
+ + + + + +
+
+
+ + /localization/yabloc/map/height + +
+
+
+
+ + /localization/yabloc/map/height + +
+
+ + + + + +
+
+
+ + /localization/yablocmap/ll2_road_marking + +
+
+
+
+ + /localization/yablocmap/ll2_road_marking + +
+
+ + + + + +
+
+
+ + /localization/yabloc/imgproc/undistorted/camera_info + +
+
+
+
+ + /localization/yabloc/imgproc/undistorted/camera_info + +
+
+ + + + + + + +
+
+
+ + /localization/yabloc/ + +
+ + imgproc/undistorted/image_raw + +
+
+
+
+ + /localization/yabloc/... + +
+
+ + + + +
+
+
+ undistort +
+
+
+
+ + undistort + +
+
+ + + + +
+
+
+ ground_server +
+
+
+
+ + ground_server + +
+
+ + + + +
+
+
+ ll2_decompose +
+
+
+
+ + ll2_decompose + +
+
+ + + + + +
+
+
+ + /localization/pf/predicted_particles + +
+
+
+
+ + /localization/pf/predicted_particles + +
+
+ + + + + +
+
+
+ + /localization/pose_estimator/pose_with_covariance + +
+
+
+
+ + /localization/pose_estimator/pose_with_covariance + +
+
+ + + + + + +
+
+
+ particle_predictor +
+
+
+
+ + particle_predictor + +
+
+ + + + + + +
+
+
+ pf/ +
+ gnss_particle_corrector +
+
+
+
+ + pf/... + +
+
+ + + + + +
+
+
+ + /localization/pf/weighted_particles + +
+
+
+
+ + /localization/pf/weighted_particles + +
+
+ + + + +
+
+
+ pf/ +
+ camera_corrector +
+
+
+
+ + pf/... + +
+
+ + + + + +
+
+
+ + /localization/yabloc/imgproc/projected_lsd_cloud + +
+
+
+
+ + /localization/yabloc/imgproc/projected_lsd_cloud + +
+
+ + + + +
+
+
+ segment_filter +
+
+
+
+ + segment_filter + +
+
+ + + + + +
+
+
+ + /localization/yabloc/ +
+ imgproc/segmented_image +
+
+
+
+
+ + /localization/yabloc/... + +
+
+ + + + +
+
+
+ graph_segment +
+
+
+
+ + graph_segment + +
+
+ + + + + +
+
+
+ + /localization/yabloc/imgproc/lsd_cloud + +
+
+
+
+ + /localization/yabloc/imgproc/lsd_cloud + +
+
+ + + + +
+
+
+ line_segment_ +
+ detection +
+
+
+
+ + line_segment_... + +
+
+ + + + + +
+
+
+ + /localization/yabloc/pf/pose + +
+
+
+
+ + /localization/yabloc/pf/pose + +
+
+ + + + +
+
+
+ + /imgproc + +
+
+
+
+ + /imgproc + +
+
+ + + + +
+
+
+ + /map + +
+
+
+
+ + /map + +
+
+ + + + +
+
+
+ + /pf + +
+
+
+
+ + /pf + +
+
+ + + + + +
+
+
+ /localization/twist_estimator/twist_with_covariance +
+
+
+
+ + /localization/twist_estimator/twist_with_covariance + +
+
+ + + + +
+
+
+ /localization +
+ /twist_estimator +
+ /gyro_odometer +
+
+
+
+ + /localization... + +
+
+ + + + + +
+
+
+ /sensing/gnss/pose_with_covariance +
+
+
+
+ + /sensing/gnss/pose_with_covariance + +
+
+ + + + +
+
+
+ /sensing +
+ /gnss +
+ /gnss_poser +
+
+
+
+ + /sensing... + +
+
+ + + + +
+
+
+ /map +
+ /lanelet2_map_loader +
+
+
+
+ + /map... + +
+
+ + + + + + + +
+
+
+ camera_pose_ +
+ initializer +
+
+
+
+ + camera_pose_... + +
+
+ + + + +
+
+
+ semantic_ +
+ segmentation +
+
+
+
+ + semantic_... + +
+
+ + + + + + + + +
+
+
+ /initialpose3d +
+
+
+
+ + /initialpose3d + +
+
+ + + + +
+
+
+ /localization +
+ /util +
+ /pose_initializer +
+
+
+
+ + /localization... + +
+
+ + + + +
+
+
+ + /initializer + +
+
+
+
+ + /initializer + +
+
+
+ + + + + Text is not SVG - cannot display + + + +
\ No newline at end of file diff --git a/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_image_description.png b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_image_description.png new file mode 100644 index 00000000000..d76977535d3 Binary files /dev/null and b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_image_description.png differ diff --git a/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_principle.png b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_principle.png new file mode 100644 index 00000000000..0d7a8294bab Binary files /dev/null and b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_principle.png differ diff --git a/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_rviz_description.png b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_rviz_description.png new file mode 100644 index 00000000000..bd931b139f9 Binary files /dev/null and b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_rviz_description.png differ diff --git a/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_thumbnail.jpg b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_thumbnail.jpg new file mode 100644 index 00000000000..79b6a278ff3 Binary files /dev/null and b/docs/how-to-guides/others/images/yabloc-integration-guide/yabloc_thumbnail.jpg differ diff --git a/docs/how-to-guides/others/yabloc-guide.md b/docs/how-to-guides/others/yabloc-guide.md new file mode 100644 index 00000000000..f4ddb9242ec --- /dev/null +++ b/docs/how-to-guides/others/yabloc-guide.md @@ -0,0 +1,97 @@ +# Using YabLoc in Autoware + +## What is YabLoc? + +YabLoc is a camera-based localization method. +It estimates position by matching road surface markings extracted from images with a vector map. +Point cloud maps and LiDAR are not required. +YabLoc enables users localize with vehicles that are not equipped with LiDAR and in environments where point cloud maps are not available. + +### Principle + +The diagram below illustrates the basic principle of YabLoc. It extracts road surface markings by masking the line segments using the road area obtained from graph-based segmentation. The red line at the center-top of the diagram represents the line segments identified as road surface markings. YabLoc transforms these segments for each particle and determines the particle's weight by comparing them with the cost map generated from Lanelet2. + +![node_diagram](images/yabloc-integration-guide/yabloc_principle.png) + +### YabLoc Packages + +- [yabloc_common](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc/yabloc_common/README.md) +- [yabloc_imgproc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc/yabloc_imgproc/README.md) +- [yabloc_particle_filter](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc/yabloc_particle_filter/README.md) +- [yabloc_pose_initializer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc/yabloc_pose_initializer/README.md) + +## How to Launch + +When launching autoware, if you set `localization_mode:=yabloc` as an argument, YabLoc will be launched instead of NDT. +By default, `localization_mode` is `ndt`. + +A sample command to use YabLoc is as follows + +```shell +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:=$HOME/autoware_map/sample-map-rosbag\ + vehicle_model:=sample_vehicle \ + sensor_model:=sample_sensor_kit \ + localization_mode:=yabloc +``` + +## Architecture + +![node_diagram](images/yabloc-integration-guide/yabloc_architecture.drawio.svg) + +### Inputs + +| Name | Type | +| ----------------------------------------------------- | ---------------------------------------------- | +| `/sensing/camera/traffic_light/image_raw/compressed` | `sensor_msgs/msg/CompressedImage` | +| `/sensing/camera/traffic_light/camera_info` | `sensor_msgs/msg/CameraInfo` | +| `/sensing/gnss/pose_with_covariance` | `geometry_msgs/msg/PoseWithCovarianceStamped` | +| `/localization/twist_estimator/twist_with_covariance` | `geometry_msgs/msg/TwistWithCovarianceStamped` | +| `/map/vector_map` | `autoware_auto_mapping_msgs/msg/HADMapBin` | +| `/tf_static` | `tf2_msgs/msg/TFMessage` | +| `/initialpose3d` | `geometry_msgs/msg/PoseWithCovarianceStamped` | + +### Outputs + +| Name | Type | +| ----------------------------------------------------- | --------------------------------------------- | +| `/localicazation/pose_estimator/pose_with_covariance` | `geometry_msgs/msg/PoseWithCovarianceStamped` | + +## Visualization + +### Core visualization topics + +These topics are not visualized by default. + + + +| index | topic name | description | +| ----- | ------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| 1 | `/localicazation/yabloc/pf/predicted_particle_marker` | particle distribution of particle fitler. Red particles are probable candidate. | +| 2 | `/localicazation/yabloc/pf/scored_cloud` | 3D projected line segments. the color indicates how well they match the map. | +| 3 | `/localicazation/yabloc/imgproc/lanelet2_overlay_image` | overlay of lanelet2 (yellow lines) onto image based on estimated pose. If they match well with the actual road markings, it means that the localization performs well. | + +### Image topics for debug + +These topics are not visualized by default. + + + +| index | topic name | description | +| ----- | ---------------------------------------------------------------- | ------------------------------------------------------------------------------- | +| 1 | `/localicazation/yabloc/pf/cost_map_image` | cost map made from lanelet2 | +| 2 | `/localicazation/yabloc/pf/match_image` | projected line segments | +| 3 | `/localicazation/yabloc/imgproc/image_with_colored_line_segment` | classifified line segments. green line segments are used in particle correction | +| 4 | `/localicazation/yabloc/imgproc/lanelet2_overlay_image` | overlay of lanelet2 | +| 5 | `/localicazation/yabloc/imgproc/segmented_image` | graph based segmentation result | + +## Limitation + +- **Running YabLoc and NDT simultaneously is not supported.** + - This is because both are computationally expensive. + - Also, in most cases, NDT is superior to YabLoc, so there is less benefit to running them at the same time. +- It does not estimate roll and pitch, therefore some of the perception nodes may not work well. +- It doesn't support multiple cameras now. But it will in the future. +- In places where there are few road surface markings, such as intersections, the estimation heavily relies on GNSS, IMU, and vhielce twist. +- If the road boundary or road surface markings are not included in the Lanelet2, the estimation fails. +- The sample rosbag provided in the autoware tutorial does not include images, so it is not possible to run YabLoc with it.