c_cpp 从欧拉角到轴旋转

Posted 2021-05-14

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