Bad Loop Closure with Lidar + Wheel Odometry Setup.

[imran05]

I am using two 2D LiDARs and merging their scans using the ira_laser_tools package.

However, I am having issues with SLAM and loop closure. The generated map gradually becomes distorted as the robot moves. Walls appear duplicated and become thicker over time, which suggests that the scans are not aligning correctly.

I have attached the rosbag file for reference. For mapping, I am using the RTAB-Map demo_hector_mapping launch file with:

odom_guess:=true
rtabmap.png
bag files: https://drive.google.com/drive/folders/1wIXahX27JkcQyn3cTiB7pOJVkF07FUdF?usp=sharing

Could you analyze the rosbag and help identify the cause of the poor loop closure and map duplication? I would also appreciate any recommendations for improving scan alignment and mapping performance when using merged data from two 2D LiDARs.

[matlabbe]

There is something off with the two laser scans. The /scan_multi has some double wall effect already, meaning the two scans are not correctly combined together. Here an example where red is left scan and white is right scan:


The small dots are the combined /scan_multi.

Are all your sensors synchronized on same clock? Is it wheel odometry?

I would recommend to try with a single scan instead of combining both, to see if you still have double wall effect.

Don't remember if ira laser tools package deals with motion while the lidar is rotating, to actually deskew the lidar and assemble two lidars appriopriatly. I tried with our own packages, that handle deskewing, but I still see double walls. That could mean there is a clock synchronization issue. Here what I tried:

ROS_NAMESPACE=LeftLidar rosrun rtabmap_util lidar_deskewing _fixed_frame_id:=odom input_scan:=rplidarNode/scan _wait_for_transform:=0.1

ROS_NAMESPACE=RightLidar rosrun rtabmap_util lidar_deskewing _fixed_frame_id:=odom input_scan:=rplidarNode/scan _wait_for_transform:=0.1

rosrun rtabmap_util point_cloud_aggregator cloud1:=/LeftLidar/rplidarNode/scan/deskewed cloud2:=/RightLidar/rplidarNode/scan/deskewed _fixed_frame_id:=odom

In red is your /scan_multi, in green is the output of point_cloud_aggregator, in particular when the robot is rotating it is worst.


I tried to deskew without the odometry frame, by using a constant velocity model, which can be done implicilty with icp_odometry:

rosrun rtabmap_odom icp_odometry scan:=/LeftLidar/rplidarNode/scan _deskewing:=true _frame_id:=base_footprint _publish_tf:=false

The output topic of the original scan deskewed is /odom_filtered_input_scan, in purple here:


It seems using a constant velocity model is better than using odometry. I suspect your odometry is not synchronized correctly with the computer's clock, or some delay is missing, because you should normally get better deskewing with wheel odometry (with IMU even better) than a constant velocity model.

Here is a similar comparison than the gif above, using icp_odometry for deskewing:

[imran05]

Dear @matlabbe,

Thank you for your detailed answer.

Yes, my sensors are synchronized, and I am using wheel odometry. I have verified the wheel odometry, and its error is less than 1%.

I also tested using only a single LiDAR, but I still observe the double-wall effect, especially when the robot is turning. The odometry appears to be properly synchronized as well. When I check the delays, the odometry delay is 0, while the scan delay is around 0.1 seconds, which may be expected since the LiDAR is running at 10 Hz.

Is there any parameter in RTAB-Map that controls the map update rate or scan integration while the robot is rotating? Or do you have any other suggestions? The issue seems to occur only during turns, while the mapping looks much better when the robot is moving straight.

Thank you

[matlabbe]

In that image I posted above



We can see the lidar shaking left and right even when the odometry seems to produce a perfectly straight trajectory. Why? Is the lidar firmly fixed to the frame? is the frame of the robot flexible?

If there is a clock synchronization / wrong delay issue, it should appear more easily when the robot is rotating on place. The double wall effect would happen more often when it is rotating. If your odometry is really 1 % error (on rotation?) and odometry clock is in sync with computer/lidar clock, normally the deskewing should be almost perfect when rotating on-place. See for example this relatively extreme case: https://www.youtube.com/watch?v=sJdzil70yGQ .

You can play with this:

ROS_NAMESPACE=LeftLidar rosrun rtabmap_util lidar_deskewing _fixed_frame_id:=odom input_scan:=rplidarNode/scan _wait_for_transform:=0.1

as long as you can clearly see the walls not aligning correctly in the deskewed scan while the robot is rotating on place, there is something wrong with the timings somewhere, and rtabmap's back-end cannot correct these afterwards. The back-end assumes that the scans are already deskewed.