Hierarchical optimization on manifolds for online 2D and 3D mapping
Treemap: An O(log n) algorithm for indoor simultaneous localization and mapping