aruco markers rvec to quaternion and +-180 degrees flip

edit

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]