Fast radius neighbor search with an Octree (ICRA 2015) - View it on GitHub