Map rotation issue with scan-matching

[ygoumaz]

Required Info:

• Operating System: Ubuntu 22.04
• Installation type: binaries
• ROS Version: ROS 2
• Version or commit hash: 2.6.3
• Laser unit: Hokuyo

Steps to reproduce issue

In the webots_ros2 package, we use the SLAM toolbox package to generate a map with a PAL Robotics TIAGo++ robot. When using the scan-matching option of the package, the map completely rotates (in a very incorrect way) after a few seconds.

You can reproduce the problem:

• Install webots_ros2 2023.0.2 (binaries) with sudo apt install ros-humble-webots-ros2
• Run the TIAGo example with slam-toolbox: ros2 launch webots_ros2_tiago robot_launch.py rviz:=true slam:=true
• Move the robot backwards with ros2 run teleop_twist_keyboard teleop_twist_keyboard.
• After a few seconds the map completely drifts.

With a very well calibrated odometry, it works without scan-matching but it ends up drifting after a few minutes. Scan-matching would be great to compensate for this drift, but it is impossible to use because of this bug.

Expected behavior

Scan-matching should work normally without big map changes and estimated position change. It should match the accurate /odom topic.

Actual behavior

The odometry given in /odom is very accurate, but scan-matching gives a big incorrect position estimate and the map is wrong.

https://user-images.githubusercontent.com/61198661/225621605-e24c6a75-cd4f-46d9-9eff-c9a767fa3eba.mp4

[SteveMacenski]

This is a not-so-uncommon issue with trivialized environments like the one you show in your video which is not reflective of reality or the performance you should expect on a real system in the real world.

The issue you're seeing is that there are either (1) too few features to do effective scan-to-scan matching on (or your odometry is really truly terrible) which is the most common reason I see this (2) you've messed with the configs greatly and essentially de-tuned the system I worked reasonably hard to provide a reasonable out of the box baseline with - in which case reverting parameters and starting tuning again might be good.

If you had a real environment, you would not see that behavior, its an artifact of the overwhelming "perfectness" of the simulated environment paired with very few distinctive features to lock the system into a solution. I've personally mapped spaces up to 200,000 sqft with this system and the provided default parameters without a hiccup and I know its used in the real-world by dozens of other companies in similarly scaled and varying environments - so I'm pretty confident in its functionality. There is a quirk with optimization and scan-matching based SLAMs where this can occur if you create such trivial, featureless, perfect environments, but that's an issue in simulation only. Even then, if you made a more realistic environment that's not just an empty room, it quickly improves in performance if it has a few unique items to latch onto.

We don't use the /odom topic for odometry, we use TF, in case that distinction is important to make.

So, I don't think there' anything actionable on my end with this - unless you're saying that you have this issue in real life or even after creating a sufficiently "real" environment. Even polling GitHub for realistic simulation worlds, this works well on those which I've tried (e.g. AWS warehouse, AWS hospital, willow garage, etc)

[ygoumaz]

Thank you for the explanation. It makes sense that the simulated environment is too trivial and lacks feature for the scan-matching to work correctly. I will try to make it more realistic with distinguishable features.

Concerning the parameters, I am using the default ones of the package, so I don't think it has anything to do with it.

I was using the /odom topic to calibrate the odometry but it is not relevant here. I just misunderstood what SLAM Toolbox was using as odometry reference.

[SteveMacenski]

Ok!