Sparse octree algorithms for scalable dense volumetric tracking and mapping