Implemented point to plane ICP but doesn't give very good results yet