introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
916 stars 549 forks source link

Tune localization with 2d lidar. #1164

Open brayanpa opened 1 month ago

brayanpa commented 1 month ago

Hello,

I have been working on rtabmap only with 2d lidar and external marker detection. Mapping is working perfectly, however, I ran into some challenges during localization.

If I set the robot's position with the /initialpose topic, the localization is immediately corrected and localized perfectly, but if the odometry drifts, it takes a long time to correct, causing the navigation to fail.

One difference I've noticed is that when using the /initialpose topic, the localization_pose topic shows a high covariance, but when the odometry drifts the covariance stays low.

If you could give me some guidance on what parameters to play with that would be fantastic.

These are the parameters saved in my database. params.txt

This is the type of situation where localization takes a long time to correct.

image

matlabbe commented 3 weeks ago

Increasing Icp/MaxCorrespondenceDistance between 0.5 and 1 meter should be enough to correct the drift in your picture.

Other option is to use icp_odometry to decrease the odometry drift (using guess_frame_id with your current odom frame, and Reg/Force3DoF=true) at disadvantage of using more CPU.

The localization covariance increases depending on covariance set in odometry topic (and reduce on loop closures). If the robot is drifting more in reality than the localization pose covariance is showing, then it could mean the covariance set in input odometry is underestimated.