Adds octomap functions from stereo-imu-obstacles as its own class, which should be really nice to link against.