Revision history [back]

I'm not sure you'll find an open source implementation, but I can tell you from experience its been done dozens (if not hundreds) of times in academia and industry. If you're just looking for something to get started with, google cartographer has 3D support. You can use this to generate a map via SLAM and with different parameters localize within a map. This will get you to the "where am I" and "where is traversable" answers. Next is "how do I get from A to B" which you could very conceivably use navigation as a reference guide to.