Map produced by slam_toolbox diverges when there is odom drift and high density of graph nodes
Hello all,
Setup: Ubuntu 20.4, ROS2 foxy, slam_toolbox installed as a debian package, gazebo11.
I am running async_slam_toolbox_node together with gazebo simulation and an rc car. The laser scan is published by a gazebo_ros_ray_sensor plugin and odometry is obtained by integrating the velocities published by /model_states topic ,which is available via gazebo_ros_state plugin, and by adding some Gaussian noise to simulate drift.
When I start the simulation and I launch the async_slam_toolbox I drive the car around one of the obstacles, as shown below.
Initially everything seems fine, the graph is building up and the map is consistent. However, after a while the map starts to diverge as shown in the second gif.
I would like to know why the odom drift is causing this issue (I tried to build the map without adding the noise to the odometry and the map was consisent, so I conclude that this is the root cause). As far as I know, SLAM front-end should add the node in the graph only if the robot moved or rotated more than the given threshold. Then, the node's position and orientation is adjusted based on the laser scan (scan matching between two subsequent scans, ICP as an example) and odometry information. When there's a loop closure the graph is optimized such that the current error of all nodes' positions and orientations is minimized based on the virtual measurements. Also I thought that there's some kind of hierarchy in the graph, such that no new nodes are inserted in the places that are already marked as visited. Therefore, based on those points I do not understand why odometry drift is causing this problem.
When I observe how the map is created I have a feeling that the new nodes are inserted into the graph based on the odometry only and that there is some kind of ICP step missing. How can I account for the drift error such that the generated map is consistent? I would like to know if it is expected that the odometry messages are drift-error free and if it makes sense to fuse this information together with laser scans before I provide it to SLAM (I thought it is done by SLAM front-end).
Please let me know if there's anything that I can add that would help resolve this issue, but I assume for now that there's a problem with my understanding of how SLAM works under the hood.
Edit: I spent some time investigating the problem further. In the figure below the red dots are showing the nodes of the graph. I have a feeling that there should not be such a high density of nodes, so I would expect that the new nodes should not be added in the previously seen places, or if so, old nodes should be replaced by new nodes. Is there a way not to add new nodes in the areas with high density of nodes - for instance, by changing some ...
Hi there! Unfortunately I can't answer your question. However, I would like to know how you have added the odometry drift you are talking about. I am also testing out several SLAM algorithms but I am using the diff_drive gazebo plugin for movement which does not allow me to change it's native covariance matrix. Did you use a different kind of plugin (Maybe created your own)?
Hi @psilva. I am not using any of the gazebo plugins to publish to /odom topic. I created my own publisher with the aid of the gazebo_ros_state plugin. It offers /model_states topic in which the true velocities and positions of the model are published. I read the 3 velocities (x, y linear, and z angular) and perturb it with some gaussian noise. Then the velocities are integrated and I publish the obtained positions via tf and /odom topic. I am not fully sure if this solution produces results similar to the ones observed by the drift error though.
I know this still has nothing to do with the original question but do you mind sharing the code for how you are achieving that? I'm trying to do a similar thing as stated in here but I'm using the /odom topic coming from the gazebo diff_drive plugin and then adding noise to that topic and republishing it in the tf tree as noisy_odom -> odom -> base_footprint. However, my robot model is jumping all over the place especially as I get further away from original odom frame.