On Closed-Form Formulas for the 3-D Nearest Rotation Matrix Problem

Abstract

The problem of restoring the orthonormality of a noisy rotation matrix by finding its nearest correct rotation matrix arises in many areas of robotics, computer graphics, and computer vision. When the Frobenius norm is taken as the measure of closeness, the solution is usually computed using the singular value decomposition (SVD). A closed-form formula exists but, as it involves the roots of a polynomial of third degree, it is assumed to be too complicated and numerically ill-conditioned. In this article, we show how, by carefully using some algebraic recipes scattered in the literature, it is possible to derive a simple and yet numerically stable formula for most practical applications. Moreover, by relying on a result that permits obtaining the quaternion corresponding to the sought optimal rotation matrix, we present another closed-form formula that provides a good approximation to the optimal one using only the elementary algebraic operations of addition, subtraction, multiplication, and division. These two closed-form formulas are compared with respect to the SVD in terms of accuracy and computational cost.