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-
In the meantime:
#include
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 :)