Revised error function, added flipping to icp approach.
Added 'grasp-points' special label to point-mapping, and fixed related bugs.