slam running in 5-6ms with optimization level 2. motion model must be fixed tho.