2D Match & Transformation Bugfix.

Some unusable ICP code. Don't care about it.