Fix the bug that was causing innacurate results.

It was really stupid, actually. Now slam works as expected.