Fixed a longstanding bug in the ICP implementation. Use world instead of
local coordinates, otherwise it always ends up being just one iteration