Map optimizer + obstacle expander optimized