add slam optimization