Improving Automated Insertion Applications by Restoring Rigid-Body Condition in Point-Based Registration