Graph SLAM with signed distance function maps on a humanoid robot

For such common tasks as motion planning or object recognition robots need to perceive their environment and create a dense 3D map of it. A recent breakthrough in this area was the KinectFusion algorithm [16], which relies on step by step matching a depth image to the map via ICP to recover the sensor pose and updating the map based on that pose. In so far it ignores techniques developed in the graph-SLAM area such as fusion with odometry, modeling of uncertainty and distributing an observed inconsistency over the map. This paper presents a method to integrate a dense geometric truncated signed distance function (TSDF) representation as KinectFusion uses with a sparse parametric representation as common in graph SLAM. The key idea is to have local TSDF sub-maps attached to reference nodes in the SLAM graph and derive graph-SLAM links via ICP by matching a map to a depth image. By moving these reference nodes according to the graph-SLAM estimate, the overall map can be deformed without touching individual sub-maps so that re-building of sub-maps is only needed in case of significant deformation within a sub-map. Also, further information can be added to the graph as common in graph SLAM. Examples are odometry or the fact that the ground is roughly but not exactly planar. Additionally, the paper proposes a modification of the KinectFusion algorithm to improve handling of long range data by taking the range dependent uncertainty into account.