Orthonormalize a Rotation Matrix

If you use a 3×3 R matrix to store the result of the multiplication of a series of rotation transformations, it could be the case that sometimes you end up with a matrix that is not orthonormal (i.e. det(R) != 1 and R.R' != eye).

In such cases, you need to re-orthonormalize the rotation matrix, which can be done in either of the two following ways:

1. Use the SVD decomposition as follows (MATLAB syntax):

   [u s vt] = svd(R);
   R = u * vt';

2. Alternatively, you can express the rotation matrix as a quaternion, normalize the quaternion, then convert the quaternion back to the rotation matrix form. In MATLAB, you could do this:

   R = quat2rotm(quatnormalize(rotm2quat(R)));

   Note that the above syntax requires MATLAB’s robotics toolbox.