Towards Optimal 3D Reconstruction And Semantic Mapping