Eigen - re-orthogonalization of the rotation matrix

Question

After multiplying a large number of rotation matrices, the final result may no longer be a valid rotation matrix due to rounding problems (deortogonalized)

One way to re-orthogonalize is as follows:

1. Convert rotation matrix to axial angle representation ( link )
2. Convert back axial angular to rotation matrix ( link )

Is there something similar in the Eigen library that hides all the details? Or is there a better recipe?

This procedure must be handled with caution due to special cases where Eigen offers the best tool for this, it would be great.

Answer 1

I do not use Eigen and did not bother looking for an API, but here is a simple, inexpensive and stable procedure for re-orthogonalizing a rotation matrix. This orthogonalization procedure is taken from the IMU Cosine Matrix Direction: Theory William Premerlani and Paul Bager; equations 19-21.

Let x, yand zbe the row vectors of the (slightly spoiled) rotation matrix. Let error=dot(x,y)where dot()- a point product. If the matrix is ​​orthogonal, the point product xand y, i.e. errorwill be zero.

error x y: x_ort=x-(error/2)*y y_ort=y-(error/2)*x. z_ort=cross(x_ort, y_ort), x_ort y_ort.

x_ort, y_ort z_ort, .

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

.

API, Eigen. , , . , ; .

Answer 2

QR- , Q. , , Q, R ( 1, ).

Q QP, P - . QP SVD. USV ', Q = UV'.

Answer 3

. :

M = UΣV - M, R = UV.

Σ . R orthogonal, . , R , M, .

Answer 4

:

#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;

, ( , ?),

, / :)

Answer 5

An alternative is to use Eigen::Quaternionto represent your rotation. This is much easier to normalize, and products rotation*rotationtend to be faster. If you have many products rotation*vector(with the same matrix), you must locally convert the quaternion to a 3x3 matrix.