14

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-orthogonalize is to follow these steps:

  1. Convert the rotation matrix to an axis-angle representation (link)
  2. Convert back the axis-angle to a rotation matrix (link)

Is there something in Eigen library that does the same thing by hiding all the details? Or is there any better recipe?

This procedure has to be handled with care due to special singularity cases, so if Eigen provides a better tool for this it would be great.

dim_tz
  • 1,381
  • 4
  • 19
  • 35

5 Answers5

14

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.

varagrawal
  • 2,689
  • 3
  • 24
  • 35
Ali
  • 54,239
  • 29
  • 159
  • 255
  • 1
    It should be noted that the normalization procedure here uses a Taylor expansion to approximate vector magnitude. If high precision is needed or the matrix is far from orthonormal then another method should be used. – Praxeolitic Apr 10 '16 at 20:18
  • 1
    @Praxeolitic I already did: See my [comment from 2 years ago](http://stackoverflow.com/questions/23080791/eigen-re-orthogonalization-of-rotation-matrix/23082112?noredirect=1#comment35363694_23083722). – Ali Apr 11 '16 at 10:13
  • Anonymous downvotes aren't helping anybody. What's wrong with the answer? – Ali Oct 14 '18 at 19:45
  • Your code snippet has some computational errors. It should be `x_new = 0.5*(3-dot(x_ort,x_ort))*x_ort`. What your code is doing is `x_new = 1/ (2*(3-dot(x_ort,x_ort))*x_ort)` Note how the parens are rearranged due to operation priority. I am guessing people copy-pasted your code and it didn't work for them and they didn't debug. – varagrawal Oct 04 '20 at 23:44
11

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'.

Lutz Lehmann
  • 23,321
  • 2
  • 19
  • 49
  • 1
    If definitely gives better results than what you get with the procedure in my answer but the price is the significantly higher computation costs. – Ali Apr 17 '14 at 10:36
  • Yes, one should do what is easiest. The question did not give a dimension, so it could be 3D as in your answer or something different. [[Easy in 3D is also the QR-decomposition using Givens rotations (which coincidentally use the Euler angle representation of the rotation matrix). Edit: I see that this was mentioned in the question, so 3D is it.]] – Lutz Lehmann Apr 17 '14 at 10:46
  • In any case, at least I upvoted your answer. :) (Although the OP should have done that already in my opinion.) – Ali Apr 17 '14 at 11:36
  • Hi Lutzl, it's easy to do QR decomp and get Q matrix in Eigen: `rotation.householderQr().householderQ()`. I'm not familier with houseHolder method however. Is that good enough? Also could you please elaborate in your answer about "check and correct" and "negate the corresponding column" as extra step? Any references or code samples would be great as well! – Shital Shah Dec 30 '16 at 23:02
  • Test it by applying the QR decomposition to a unit matrix `I`. It is possible that the result is `[Q, R] = [-I, -I]`. This happens because the Householder reflections are chosen to be the most stable, i.e., avoiding divisions by zero or near zero. – Lutz Lehmann Dec 31 '16 at 07:31
4

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.

rwp
  • 1,628
  • 2
  • 14
  • 27
DavidS
  • 454
  • 4
  • 16
  • This essentially is one way to compute the polar decomposition, M=RP, P=V^TΣV a s.p.d matrix, which gives the closest orthogonal matrix R to the given M. The cost is the non-trivial nature of the SVD algorithm. – Lutz Lehmann Oct 14 '18 at 13:02
1

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 :)

dim_tz
  • 1,381
  • 4
  • 19
  • 35
  • 1
    *"Thanks for the answers so far!"* Here at Stackoverflow we don't say thank you, we upvote and/or accept answers instead. *"how can one be sure that he takes care of all singularities?"* I guess it assumes that `rrr` *is in fact* a rotation matrix. If it isn't (and that's what you problem is, your rotation matrix is messed-up), then it most likely does something wrong. This approach seems to be more like an ad-hoc hack than a solution. – Ali Apr 17 '14 at 10:33
  • 1
    It is not really necessary to compute the angles, one can just multiply the axis rotation matrices back together without worrying about quadrants and singularities. This is then the QR decomposition using the Givens rotations method. – Lutz Lehmann Apr 17 '14 at 10:49
  • 2
    And if you want to improve the A=QR decomposition, check if the off-diagonal part of R is small and use Q*(I+(R-R')/2) as a better approximation. If (R-I) is somewhat larger, use the better approximation I+(R-R')/2+(R-R')^2/8 or the exact [Rodrigues formula](https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula) to compute the matrix exponential of (R-R')/2. (R'=transpose of R, first identify k from the matrix, then apply formula.) – Lutz Lehmann Apr 17 '14 at 11:09
1

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.

chtz
  • 16,028
  • 4
  • 23
  • 52