Real-Time Dense Multi-Scale Workspace Modeling on a Humanoid Robot

Without a precise and up-to-date model of its environment a humanoid robot cannot move safely or act usefully. Ideally, the robot should create a dense 3D environment model in real-time, all the time, and respect obstacle information from it in every move it makes as well as obtain the information it needs for fine manipulation with its fingers from the same map. We propose to use a multi-scale truncated signed distance function (TSDF) map consisting of concentric, nested cubes with exponentially decreasing resolution for this purpose. We show how to extend the KinectFusion real-time SLAM algorithm to the multi-scale case as well as how to compute a multi-scale Euclidean distance transform (EDT) thereby establishing the link to optimization-based planning. We overcome the inability of KinectFusion's localization to handle scenes without enough constraining geometry by switching to mapping-with-known-poses based on forward kinematics. The latter is always available and we know when it is precise. The resulting map has the desired properties: It is computed in real-time (7.5 ms per depth frame for a (8 m)(3) multi-scale TSDF volume), covers the entire laboratory, does not depend on scene properties (geometry, texture, etc.) and is precise enough to facilitate grasp planning for fine manipulation tasks - all in a single map.