fix slam optimization