introlab
Understanding RTAB's capabilities with just 3D Lidar
Hello,
I've been experimenting with RTAB and its ability to handle two lidars, and I had the following questions:
- Once a map is made, is it possible to determine the robot's location in the map on startup?
- Does an initialpose need to be set? or can RTAB match the current lidar scans with the past map to get a global pose estimate?
Basic test procedure is: build a map using a ros bag, and than changing rtab to loclization mode and running a different rosbag on it. Note the two rosbags are recorded in the same location, with the bag used for mapping being the bigger of the two.
Thanks
RTAB-Map cannot globally re-localize with only a LiDAR. You have to send a 2d pose estimate under /initialpose topic to initialize the first pose, afterwards, RTAB-Map can locally localize. Here an example with husky demo with camera disabled. I launched the demo first in mapping mode, moved a little and then relaunched in localization mode. In the gif below, it is after relaunching in localization mode. If you add /localization_pose topic in RVIZ, the whole screen will be pink, because the localization covariance is set to 9999, meaning it is lost. By using 2D pose Estimate button, which sends initialpose topic, we can teleport the robot in the map. RTAB-Map still try to localize locally, so as soon we send an initial pose that is close to reality, RTAB-Map will snap back at the right position in the map, shrinking the localization pose's covariance.
Hi @matlabbe, thanks for the clarification! I suspected so the more I dug deeper into the code.
As an addition, I was wondering about apriltag usage in RTAB. It seems that if we want to use the functionality, then a complete recompile of the code is needed. Is there docs/samples pointing towards using apriltags? Is global relocalization possible if an apriltag is detected?
We can build rtabmap_slam with apriltag_msgs support, then we could have an external node publishing on tag_detections (type apriltag_msgs/msg/AprilTagDetectionArray) input topic of rtabmap node.
Other option is to rebuild rtabmap library with OpenCV+ArUco module, then set parameter RGBD/MarkerDetection to "true" and the corresponding parameters. Except the first four parameters, all the others also affect external tag detection (tag_detections topic).
rtabmap --params | grep "Param: Marker/"
Param: Marker/CornerRefinementMethod = "0" [Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is "doCornerRefinement" parameter: set 0 for false and 1 for true.]
Param: Marker/Dictionary = "0" [Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20]
Param: Marker/Length = "0" [The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).]
Param: Marker/MaxDepthError = "0.01" [Maximum depth error between all corners of a marker when estimating the marker length (when Marker/Length is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.]
Param: Marker/MaxRange = "0.0" [Maximum range in which markers will be detected. <=0 for unlimited range.]
Param: Marker/MinRange = "0.0" [Miniminum range in which markers will be detected. <=0 for unlimited range.]
Param: Marker/Priors = "" [World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line ("id1 x y z roll pitch yaw|id2 x y z roll pitch yaw"). Example: "1 0 0 1 0 0 0|2 1 0 1 0 0 1.57" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).]
Param: Marker/PriorsVarianceAngular = "0.001" [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001" [Linear variance to set on marker priors.]
Param: Marker/VarianceAngular = "0.01" [Angular variance to set on marker detections. If Marker/VarianceOrientationIgnored is enabled, it is ignored with Optimizer/Strategy=1 (g2o) and it corresponds to bearing variance with Optimizer/Strategy=2 (GTSAM).]
Param: Marker/VarianceLinear = "0.001" [Linear variance to set on marker detections. If Marker/VarianceOrientationIgnored is enabled and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.]
Param: Marker/VarianceOrientationIgnored = "false" [When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw).]
Yes, global localization can work with apriltags/markers, or any sensor that can provide a relative pose on a topic of type apriltag_msgs/msg/AprilTagDetectionArray.
I am currently preparing a new ROS2 binary release (Humble/Jazzy/Kilted), I'll include apriltag_msgs as dependency already to avoid rebuilding the package for the tag_detections topic support.
@matlabbe one more thing. Is there a mechanism in place to trigger a request for a new initial pose? ie. localization covariance reaches some threshold, and so rtab notifies so through some topic, and we will have to pass a pose into some other topic to localize again.
Also, is there a roadmap for integrating some sort of global localization mechanism for lidar? IE. particle filter or https://arxiv.org/pdf/2310.10023
I'd be happy to help :)
Thanks for the reference. I don't have plan in the short-term to integrate a Lidar-only global localization approach. In the past, we integrated this approach (cmr_lidarloop) externally, which is an approach to compare descriptors representing different laser scans added in the map.
The 3D-BBS approach you linked could be externally integrated to rtabmap too I think, even less tightly integrated than cmr_lidarloop approach, as you would only need the assembled point cloud of the the full map once, than trigger global localization when needed, then call /rtabmap/initialpose to teleport the robot there, from there, RTAB-Map's proximity localization could handle localization afterwards.
We provide /rtabmap/localization_pose. which contains a covariance. Some external node could subscribe to it and re-trigger a global localization if the covariance increases too much or reached some pre-defined threshold, then call /rtabmap/initialpose to teleport the robot there.
Note that rtabmap_slam/rtabmap has a parameter called loc_thr, which can trigger some warnings in ros diagnostic (if you open rqt_runtime_monitor):
https://github.com/introlab/rtabmap_ros/blob/7472b019f90f08aef9fb6c8205307909231c5465/rtabmap_slam/src/CoreWrapper.cpp#L4917-L4928