欧拉角和旋转矩阵的转换（附相关代码实现）

 1、欧拉角到旋转矩阵

 代码实现：

Eigen::Matrix3d euler2Rotation( Eigen::Vector3d  eulerAngles)
{
    double roll = eulerAngles(0);
    double pitch = eulerAngles(1);
    double yaw = eulerAngles(2);

    double cr = cos(roll); double sr = sin(roll);
    double cp = cos(pitch); double sp = sin(pitch);
    double cy = cos(yaw); double sy = sin(yaw);

    Eigen::Matrix3d RIb;
    RIb<< cy*cp ,   cy*sp*sr - sy*cr,   sy*sr + cy* cr*sp,
            sy*cp,    cy *cr + sy*sr*sp,  sp*sy*cr - cy*sr,
            -sp,         cp*sr,           cp*cr;
    return RIb;
}

2、旋转矩阵到欧拉角

 代码实现：

//由旋转矩阵计算欧拉角
        private double[] rotationMatrixToEulerAngles(double[] M)
        {
            double R00 = M[0], R01 = M[1], R02 = M[2];
            double R10 = M[4], R11 = M[5], R12 = M[6];
            double R20 = M[8], R21 = M[9], R22 = M[10];

            double sy = Math.Sqrt(R00 * R00 + R10 * R10);

            bool singular = sy < 1e-6; // If

            double x, y, z;
            if (!singular)
            {
                x = Math.Atan2(R21, R22);
                y = Math.Atan2(-R20, sy);
                z = Math.Atan2(R10, R00);
            }
            else
            {
                x = Math.Atan2(-R12, R11);
                y = Math.Atan2(-R20, sy);
                z = 0;
            }
            x = x * 180.0 / Math.PI;
            y = y * 180.0 / Math.PI;
            z = z * 180.0 / Math.PI;
            double[] angle = new double[3] { x, y, z };
            return angle;
        }