3D Mapping for Indoor Navigation