introlab
LIDAR Localization Problem: Robot Fails to Correct Axis Misalignment
Hi Mathieu,
We are currently using RTAB-Map configured exclusively with LIDARs, and I am experiencing some issues where the robot struggles to localize itself. I’ve noticed this tends to happen when the walls along one axis (e.g., the Y-axis relative to base_link) align with the LIDAR. In these situations, the robot does not attempt to correct the incorrect axis.
I have attached some images and the parameters we are currently using. Any suggestions on which parameters to adjust would be greatly appreciated.
Thank you.
rtabmap_params.txt
Is there any warning on the terminal? Based on your parameters, it should "snap" on the map:
Icp/MaxCorrespondenceDistance: '1.0'
Icp/MaxTranslation: '3.0'
The correspondences should be under 1 meter in that case and the translation correction would be under 3 meters. If you can enable Debug log and copy/paste the log when the robot is in that kind of mislocalization, that would be useful.
Also make sure you don't have multiple nodes publishing the same TF.
Hi Mathieu,
Sorry for the delay, we are just getting back to this case.
We enabled debug mode and found that, in some cases, localization is rejected because the error exceeds our RGBD/OptimizeMaxError threshold:
Rejecting localization (973 <-> 663) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.711624 (edge 972->973, type=0, abs error=0.371162 m, stddev=0.100000). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.
To allow more localizations, we have increased RGBD/OptimizeMaxError to 5.0.
Additionally, we found cases where there are not enough correspondences according to the logs:
RegistrationIcp.cpp:930::computeTransformationImpl() Cannot compute transform (cor=52 corrRatio=0.135065/0.200000 maxLaserScans=385)
In this case, is it expected for correspondencesRatio to be 0.2 when our parameter is set to 0.1?
#ros2 param get /gary/nav/rtabmap/rtabmap Icp/CorrespondenceRatio
String value is: 0.1
We also noticed logs related to the number of LiDAR points:
LaserScan.cpp:332::init() The number of points (1061) in the scan is over the maximum points (162) defined by max points setting.
LaserScan.cpp:332::init() The number of points (3835) in the scan is over the maximum points (688) defined by max points setting.
Our LiDAR produces 848 points, so we would like some guidance on how the number of points is determined in these logs. Should we consider adjusting the max points setting to better match our sensor’s output?
I have also attached a full log file in case it helps with debugging.
Looking forward to your insights.
Best,
Brayan Pallares
To deal with RGBD/OptimizeMaxError, the best fix is to tune correctly the odometry twist's covariance. In your case, it seems fixed to 0.01 (sigma*sigma). The absolute error is 30 cm, if the loop closure is truly good, maybe the covariance is under estimated. If the loop closure is wrong, then RGBD/OptimizeMaxError did its job to reject it. However, sometime it is just easier to tune RGBD/OptimizeMaxError instead of the covariance, in particular if the covariance is coming from a black box.
The Icp/CorrespondenceRatio is doubled for one-to-many proximity detection. To use 0.1 for proximity detection, set Icp/CorrespondenceRatio to 0.05. Note that it is maybe a good thing to reject when correspondences ratio is under 20%.
The scan max points log is a debug log, the code is setting a max points lower than the number of points on purpose, just for the assembled scans during the one-to-many proximity detection. The final max points used would be your 845 (adjusted to voxel/decimation).
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) Memory.cpp:3188::computeIcpTransformMulti() 977 vs 895 896 897 898 899 900 901 902 903
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 977 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 902 data(0,0,1,0,0,0,0)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) Memory.cpp:3240::computeIcpTransformMulti() maxPoints from(977) = 536
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 895 data(0,0,1,0,0,0,0)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) Memory.cpp:3294::computeIcpTransformMulti() maxPoints scan(895) = 647
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 896 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 897 data(0,0,1,0,0,0,0)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) Memory.cpp:3294::computeIcpTransformMulti() maxPoints scan(897) = 661
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 898 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 899 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 900 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 901 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 902 data(0,0,1,0,0,0,0)
x[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) SensorData.cpp:550::uncompressData() 903 data(0,0,1,0,0,0,0)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) Memory.cpp:3349::computeIcpTransformMulti() assembledScan=5649 points
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) LaserScan.cpp:332::init() The number of points (5649) in the scan is over the maximum points (661) defined by max points setting.
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:327::computeTransformationImpl() Guess transform = xyz=0.296851,2.423510,0.000000 rpy=0.000000,-0.000000,-1.357244
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:328::computeTransformationImpl() Voxel size=0.050000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:329::computeTransformationImpl() PointToPlane=1
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:330::computeTransformationImpl() Normal neighborhood=5
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:331::computeTransformationImpl() Normal radius=0.000000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:332::computeTransformationImpl() Max correspondence distance=1.000000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:333::computeTransformationImpl() Max Iterations=50
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:334::computeTransformationImpl() Correspondence Ratio=0.200000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:335::computeTransformationImpl() Max translation=3.000000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:336::computeTransformationImpl() Max rotation=1.500000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:337::computeTransformationImpl() Downsampling step=1
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:338::computeTransformationImpl() Force 3DoF=true
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:339::computeTransformationImpl() Force 4DoF=false
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:340::computeTransformationImpl() Min Complexity=0.020000
u[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:341::computeTransformationImpl() libpoint
<[rtabmap-1] matcher (knn=1, outlier ratio=0.850000)
{[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:342::computeTransformationImpl() Strategy=1
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) RegistrationIcp.cpp:351::computeTransformationImpl() size before filtering, from=5649 (format=XY, max pts=661) to=536 (format=XY, max pts=845)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) util3d_filtering.cpp:85::commonFiltering() scan size=5649 format=1, step=1, rangeMin=0.000000, rangeMax=0.000000, voxel=0.050000, normalK=5, normalRadius=0.000000, groundNormalsUp=0.000000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.166) util3d_filtering.cpp:265::commonFiltering() Voxel filtering scan (voxel=0.050000 m): 5649 -> 1286 (scanMaxPts=661->150)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.170) LaserScan.cpp:332::init() The number of points (1286) in the scan is over the maximum points (150) defined by max points setting.
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.170) util3d_filtering.cpp:295::commonFiltering() Normals computed (k=5 radius=0.000000)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.170) util3d_filtering.cpp:85::commonFiltering() scan size=536 format=1, step=1, rangeMin=0.000000, rangeMax=0.000000, voxel=0.050000, normalK=5, normalRadius=0.000000, groundNormalsUp=0.000000
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.170) util3d_filtering.cpp:265::commonFiltering() Voxel filtering scan (voxel=0.050000 m): 536 -> 195 (scanMaxPts=845->307)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) util3d_filtering.cpp:295::commonFiltering() Normals computed (k=5 radius=0.000000)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) RegistrationIcp.cpp:446::computeTransformationImpl() size after filtering, from=1286 (format=XYNormal, max pts=150) to=195 (format=XYNormal, max pts=307), filtering time=0.004995s
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) RegistrationIcp.cpp:598::computeTransformationImpl() Conversion time = 0.000213 s
c[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) libpointmatcher.h:139::laserScanToDP()
c[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) libpointmatcher.h:139::laserScanToDP()
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.171) RegistrationIcp.cpp:613::computeTransformationImpl() libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.355) RegistrationIcp.cpp:616::computeTransformationImpl() libpointmatcher icp...done! T=xyz=0.806551,-0.883689,0.000000 rpy=0.000000,-0.000000,0.061332
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.355) RegistrationIcp.cpp:619::computeTransformationImpl() match ratio: 0.279160
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.356) RegistrationIcp.cpp:850::computeTransformationImpl() ICP (iterations=50) time = 0.184682 s
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.356) RegistrationIcp.cpp:876::computeTransformationImpl() Max scans=307 (from=150, to=307)
[rtabmap-1] [DEBUG] (2025-02-26 18:34:42.356) RegistrationIcp.cpp:896::computeTransformationImpl() 977->0 hasConverged=true, variance=0.015177, correspondences=22/307 (7.166124%), from guess: trans=1.015842 rot=0.061332
Looking at the code, it could be updated to use the real "max points" of the assembled scans instead of just using the max of valid points here. If the scans in the map are all coming from the same sensor, that would change nothing in your case. Having a more accurate "max points" would be useful if the map has been created from a lidar with 3000 max points, then now we try localize with a lidar with 845 max points. I still created an issue for that specific case.
cheers, Mathieu