Treemap: An O(log n) Algorithm for Simultaneous Localization and Mapping

In my Ph.D. thesis at the German Aerospace Center I developed an extremely efficient (O(log n)) algorithm for SLAM that works by dividing the map into local regions and subregions. At each level of the hierarchy each region stores a matrix representing some of the landmarks contained in this region. On the level of finest subdivision, i.e. the lowest level, these matrices are naturally small because the regions are small and contain few landmarks only. On higher levels regions are large and contain many landmarks. For keeping the matrices stored at higher levels small only those landmarks are represented being observable from outside the region. This way it is ensured that even on high levels of hierarchy each matrix represents only few landmarks and computation is efficient. A measurement is integrated into a local subregion using O(k^2) computation time for k landmarks in a subregion. When the robot moves to a different subregion a global update is necessary requiring only O(k^3 log n) computation time. Furthermore, the proposed hierarchy allows nonlinear rotation of the matrix stored at a certain region. Thereby linearization problems can be removed.

Overview talk: Treemap: An O(log n) Algorithm for Simultaneous Localization and Mapping. Please also download these two videos 1 and 2 and the two experimental videos below.

SLAM experiment 1 (small with navigation)

SLAM experiment 2 (large without navigation)

More detailed information is found in my Ph.D. thesis (2004) and in a recent Autonomous Robos, paper (2006). After my thesis we have completely redesigned the algorithm and the implementation to make it work for more general 2D or 3D variants of SLAM. You can find the more recent work here.