Static 3D Environment Map for localization and path planning

edit

You can build a static map with a 3D modelling tool (e.g. Blender, Sketchup) and then convert it to OctoMap, as detailed in these instructions: https://github.com/OctoMap/octomap/wiki/Importing-Data-into-OctoMap

There are multiple examples of path planning in OctoMap, check out the following two publications:

• Real-Time Navigation in 3D Environments Based on Depth Camera Data (Video)
• Navigation in Three-Dimensional Cluttered Environments for Mobile Manipulation (Video)

Code for 6D localization in octomap is already available in the humanoid_localization stack, with details in the paper Humanoid Robot Localization in Complex Indoor Environments (Video)

You won't get a plug-and-play solution in terms of complexity (you will probably have to read and adjust some code), but in general that has all been done before. As on why to use OctoMap: You will have a memory-efficient 3D occupancy map representation. If you environment is only small and the memory footprint does not matter, you will still have an integrated solution with most code already implemented (map building, raycasting for localization, efficient map access).