Method to Improve Point-Based Registration by Restoring Rigid-Body Condition