added reprojection into model space after icp fitting