Introduction

About Loop Closure

Loop closure is a main issue of SLAM. So, GraphSLAM is proposed for closing the loop of the map. For that, graphSLAM registers some strong features as a node. If the vehicle found a old points registered before, detect a loop. Then, it calculates the scores of how looks alike they are. If the new pointcloud and old pointcloud are similar (lower fitness score than threshold), than forcedly close the loop by moving the nodes. For moving the nodes, it use popular graph optimization library "g2o".

About Drift Error

Than, why we could not close the loop using conventional SLAM? It is because the drift error of SLAM. No matter what sensors is used for position correction, drift error always happen in SLAM. The bigger you travel, the bigger drift error occurs.

Loop Closure for Drift Error Correction

So, how to correct the drift error? The optimization of graph nodes in GraphSLAM also can correct the drift error. If loop is detected and the loop closure is performed, the drifted error will be reduced because shifted nodes will be corrected to match the old point cloud.

You can see the entire shifted nodes are moved to the correct position when loop closure.

Importance of Travel Path for SLAM

Because of the reasons above, traveling path is really really important for SLAM !!!!! If you travel a larger square, drift error will accumulated, then is is hard to detect a loop. And of course even harder to close the loop, because the fitness score will be large since the points are far away.

For example, the above GIF is simulated using "first.bag". There are just small distance from old points and new points. So the fitness score is really small because they look similar. Finally, they can merge the pointcloud successfully, and the nodes moves to the correct position. It is because the path of "first.bag" is a small closed loop !!!!!

(Note: "first.bag" and "third.bag" is recored around DGIST Campus. You can find the bag files in NAS - "/Autoware_Data/190614_MappingData/". For more information, read the REAME in that directory.)

You can see the paths of "first.bag" and "third.bag" below:

path of first.bag	path of third.bag

On the contrary, let's see the "third.bag". It succeeded to merge the loop by increasing the "score threshold" to more than 30.0 (the default is 2.0). Even the loop closure is performed, but the nodes didn't moves to correct position. You can see the path of third.bag above. The last part of the path is a perfectly flat plane. But the map created by GraphSLAM, we can see the unexpected slope. Furthermore, there is a bunch of unexpected points noise came into the road, and they disturb localization. See images below:

forcedly merged nodes	wrong slope is created

Conclusion

When running GraphSLAM with large loop path, it is true that it could be forcedly perform loop close by increasing loop closure threshold, but the map will get weird. The loop path must be as small as possible for accurate mapping !!!!!! The recommended drive path of "third.bag" must be like this :

Duplicates of the path is even recommended. So if you want to prepare a rosbag file for GraphSLAM or online GraphSLAM with real car, you must keep the loop path as small as possible. Or, create small pcd maps separately, then stitch them using algorithms such as ICP.