1 | initial version |
See Maths - AxisAngle to Quaternion.
The conversion between axis-angle and quaternion is straightforward:
#include <iostream>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace Eigen;
using std::cout;
using std::endl;
Matx41d aa2quaternion(const Matx31d& aa)
{
double angle = norm(aa);
Matx31d axis(aa(0) / angle, aa(1) / angle, aa(2) / angle);
double angle_2 = angle / 2;
//qx, qy, qz, qw
Matx41d q(axis(0) * sin(angle_2), axis(1) * sin(angle_2), axis(2) * sin(angle_2), cos(angle_2));
return q;
}
int main()
{
//Eigen
Matrix3d R;
R = AngleAxisd(0.25*M_PI, Vector3d::UnitX())
* AngleAxisd(0.5*M_PI, Vector3d::UnitY())
* AngleAxisd(0.33*M_PI, Vector3d::UnitZ());
cout << "Eigen R:\n" << R << endl;
AngleAxisd aa;
aa.fromRotationMatrix(R);
cout << "Eigen axis-angle:\n" << aa.axis()*aa.angle() << endl;
Matx33d R_cv;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
R_cv(i,j) = R(i,j);
}
}
Quaterniond q(aa);
cout << "Eigen quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << "," << q.w() << endl;
Matx31d aa_cv;
Rodrigues(R_cv, aa_cv);
cout << "OpenCV rvec: " << aa_cv.t() << endl;
Matx41d q_cv = aa2quaternion(aa_cv);
cout << "OpenCV quaternion: " << q_cv.t() << endl;
return 0;
}
Output:
Eigen R:
3.33067e-16 -1.66533e-16 1
0.968583 -0.24869 -1.66533e-16
0.24869 0.968583 3.33067e-16
Eigen axis-angle:
1.3919
1.07967
1.3919
Eigen quaternion: 0.558724, 0.433391, 0.558724,0.433391
OpenCV rvec: [1.391895819721201, 1.079665068873341, 1.391895819721201]
OpenCV quaternion: [0.5587239674393911, 0.4333907338750867, 0.558723967439391, 0.433390733875087]