Systems and methods for performing computer-assisted procedures are provided. In the systems and methods, a current rigid transformation is applied to a first set of points with respect to a second set of points to obtain a current transformed set of points. Thereafter, a linearized transformation is determined that minimizes a weighted fiducial registration error between the current transformed set and the second set. An updated rigid transformation is then obtained based on the current rigid transformation, the translation vector of the linearized transformation, and a rotation matrix that is closest to the linearized matrix of the linearized transformation, followed by applying the updated rigid transformation to the first set to obtain an updated transformed set of points. Finally, if motion associated with such updating fails to meet some criterion, the determining, obtaining, and applying are iteratively repeated using the updated rigid transformation and the updated transformed set.