Tried a couple other implementions of ICP with similar results.
libicp implementation added
It works much faster than the previous one, but it still produces an bad map. (exactly the same map as the previous implementation though.