Eigen - Re-orthogonalization of Rotation Matrix

Question

After multiplying a lot of rotation matrices, the end result might not be a valid rotation matrix any more, due to rounding issues (de-orthogonalized)

One way to re-

Answer 1

Singular Value Decomposition should be very robust. To quote from the reference:

Let M=UΣV be the singular value decomposition of M, then R=UV.

For your matrix, the singular-values in Σ should be very close to one. The matrix R is guaranteed to be orthogonal, which is the defining property of a rotation matrix. If there weren't any rounding errors in calculating your original rotation matrix, then R will be exactly the same as your M to within numerical precision.

Answer 2

You can use a QR decomposition to systematically re-orthogonalize, where you replace the original matrix with the Q factor. In the library routines you have to check and correct, if necessary, by negating the corresponding column in Q, that the diagonal entries of R are positive (close to 1 if the original matrix was close to orthogonal).

The closest rotation matrix Q to a given matrix is obtained from the polar or QP decomposition, where P is a positive semi-definite symmetric matrix. The QP decomposition can be computed iteratively or using a SVD. If the latter has the factorization USV', then Q=UV'.

Answer 3

An alternative is to use Eigen::Quaternion to represent your rotation. This is much easier to normalize, and rotation*rotation products are generally faster. If you have a lot of rotation*vector products (with the same matrix), you should locally convert the quaternion to a 3x3 matrix.

Answer 4

I don't use Eigen and didn't bother to look up the API but here is a simple, computationally cheap and stable procedure to re-orthogonalize the rotation matrix. This orthogonalization procedure is taken from Direction Cosine Matrix IMU: Theory by William Premerlani and Paul Bizard; equations 19-21.

Let x, y and z be the row vectors of the (slightly messed-up) rotation matrix. Let error=dot(x,y) where dot() is the dot product. If the matrix was orthogonal, the dot product of x and y, that is, the error would be zero.

The error is spread across x and y equally: x_ort=x-(error/2)*y and y_ort=y-(error/2)*x. The third row z_ort=cross(x_ort, y_ort), which is, by definition orthogonal to x_ort and y_ort.

Now, you still need to normalize x_ort, y_ort and z_ort as these vectors are supposed to be unit vectors.

x_new = 0.5*(3-dot(x_ort,x_ort))*x_ort
y_new = 0.5*(3-dot(y_ort,y_ort))*y_ort
z_new = 0.5*(3-dot(z_ort,z_ort))*z_ort

That's all, were are done.

It should be pretty easy to implement this with the API provided by Eigen. You can easily come up with other orthoginalization procedures but I don't think it will make a noticable difference in practice. I used the above procedure in my motion tracking application and it worked beatifully; it's both stable and fast.

Answer 5

In the meantime:

#include <Eigen/Geometry>

Eigen::Matrix3d mmm;
Eigen::Matrix3d rrr;
                rrr <<  0.882966, -0.321461,  0.342102,
                        0.431433,  0.842929, -0.321461,
                       -0.185031,  0.431433,  0.882966;
                     // replace this with any rotation matrix

mmm = rrr;

Eigen::AngleAxisd aa(rrr);    // RotationMatrix to AxisAngle
rrr = aa.toRotationMatrix();  // AxisAngle      to RotationMatrix

std::cout <<     mmm << std::endl << std::endl;
std::cout << rrr     << std::endl << std::endl;
std::cout << rrr-mmm << std::endl << std::endl;

Which is nice news, because I can get rid of my custom method and have one headache less (how can one be sure that he takes care of all singularities?),

but I really want your opinion on better/alternative ways :)