问题
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:
- Convert the rotation matrix to an axis-angle representation (link)
- 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.
回答1:
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 = 1/2*(3-dot(x_ort,x_ort))*x_ort
y_new = 1/2*(3-dot(y_ort,y_ort))*y_ort
z_new = 1/2*(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.
回答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'.
回答3:
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.
回答4:
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 :)
回答5:
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.
来源：https://stackoverflow.com/questions/23080791/eigen-re-orthogonalization-of-rotation-matrix