Towards Real Time Robot 6D Localization in a Polygonal Indoor Map Based on 3D ToF Camera Data

Abstract

This paper reports a method and results for solving the following problem: Given a 3D polygonal indoor map and a mobile robot equipped with a 3D time of flight (ToF) camera, localize at frame rate the 6D robot pose with respect to the map. To solve the problem, the polygonal map is represented for efficient usage as a solid-leaf BSP tree; at each control cycle, the 6D pose change is estimated a priori from odometry or IMU, the expected ToF camera view at the prior pose sampled from the BSP tree, and the pose change estimation corrected a posteriori by fast ICP matching of the expected and the measured ToF image. Our experiments indicate that, first, the method is in fact real-time capable; second, the 6D pose is tracked reliably in a correct map under regular sensor conditions; and third, the tracking can recover from some faults induced by local map inaccuracies and transient or local sensing errors.