Fix bugs of non-rigid registration