Optimized Octtree Datastructure and Access Methods for 3D Mapping

3D maps are becoming increasingly important for robots operating in real world scenarios. As occupancy grids are a popular standard in 2D mapping, it is a natural choice to extend them to spatial representation in 3D. But this approach suffers from large memory requirements that render 3D grids unfeasible for realistic mapping applications. A well-known alternative is quadtrees in 2D, respectively octtrees for 3D, where cells that contain identical information are collapsed into a single node in a tree. But octtrees only allow relative slow access to the spatial information stored in them. Here, a very efficient implementation of an octtree is presented. It is based on technical optimizations as well as newly introduced heuristics like the exploitation of typical access patterns. It is shown that the optimizations are indeed beneficial through experiments with real world 3D sensor data. In addition to basic access methods, a very fast nearest neighbor operation -as needed for example for registration in SLAM or map merging algorithms -for octtrees is presented. It is shown in experiments with real world data that this nearest neighbor operation outperforms a comparable operation on occupancy grids by far.