Fast, Uncertainty-Aware Proximity Queries with Lazy Search of Local 3D Data
NanoMap is an algorithm and data structure that enables uncertainty-aware proximity queries for planning.
Key features of NanoMap include:
- Incorporates frame-specific uncertainty (due to modeled local pose uncertainty) returned along with query point
- Query algorithm returns k-nearest-neighbors from most recent view of query point
- Updates pose history information 2-4 orders of magnitude faster than fusion-based voxel packages
- Particularly fast for low amounts of queries (<10,000)
git clone https://github.com/peteflorence/nanomap_ros.git catkin_make ## catkin build for catkin_tools, or catkin_make for catkin
Reccomend first testing your installation:
Then there are two examples of minimal nodes, one is for benchmarking and performs queries:
rosrun nanomap_ros benchmarking_node
The other shows how to use the
rosrun nanomap_ros visualization_node
This ros node has been tested on:
- ROS Kinetic on Ubuntu 16.04
- ROS Indigo on Ubuntu 14.04
NanoMap is particularly fast for low amounts of motion planning queries (< ~10,000), due to its low data structure build time. Fusion-based voxel structures take longer to build but have faster query times, causing them to be faster for > ~10,000 queries.
Nanomap is two to four orders of magnitude faster than these other benchmarked packages at rebuilding its data structure upon receiving updated pose history information.
(On single-core of an Intel Skylake i7. Error bars are shown as standard error of the mean. Docker image with all packages buildable inside provided: https://hub.docker.com/r/flamitdraper/mapping/ )
- Nanoflann (included as nanoflann_two.hpp)
The PCL dependency is minimal. It only uses the
pcl::PointCloud<pcl::PointXYZ>; does not use any algorithms. It would not be hard to replicate this type and remove the PCL dependency. Would gladly merge that PR, PCL is a fantastic library, but is not really used here.
The core of the code has no depdency on ROS. The only parts that have ROS dependencies are the visualization components, and the example node:
nanomap_visualizer.h / .cc
I have provided this as a ros package since that is how it has been used and tested. If you would like to use this without ros, it would just need a ros-independent CMakeLists.txt / incorporate into other build system.
The top of
#define NANOMAP_DEBUG_PRINT false. Changing this to true turns on potentially useful debug prints.
A particularly useful debug print is in
nanomap.cc. Often I will turn the first line of this function to
if (1) in order to print regular status.