jgoenetxea
2/8/2018 - 2:28 PM

From euler angles to axis-rotation

First generate the rotation matrix (XYZ in this case) and then use Rodrigues formula to transform the matrix into a vector

from_euler_to_axis_rotation
content_copyfile_download
cv::Matx33f from_euler_to_axis_rotation(const cv::Vec3f& _eulerAngles){
        cv::Matx33f X = cv::Matx33f::eye();
        cv::Matx33f Y = cv::Matx33f::eye();
        cv::Matx33f Z = cv::Matx33f::eye();

        float x = _eulerAngles[0];
        float y = _eulerAngles[1];
        float z = _eulerAngles[2];

        X(1, 1) = cos(x);
        X(1, 2) = -sin(x);
        X(2, 1) = sin(x);
        X(2, 2) = cos(x);

        Y(0, 0) = cos(y);
        Y(0, 2) = sin(y);
        Y(2, 0) = -sin(y);
        Y(2, 2) = cos(y);

        Z(0, 0) = cos(z);
        Z(0, 1) = -sin(z);
        Z(1, 0) = sin(z);
        Z(1, 1) = cos(z);

        cv::Matx33f R = Z * Y * X;
        
        cv::Vec3f rvec;
        cv::Rodrigues(R, rvec);
        
        return rvec;
    }