eigen欧拉角转旋转矩阵

/**** 3. 欧拉角 ****/ cout << endl << "********** EulerAngle **********" << endl; //3.0 初始化欧拉角(Z-Y-X，即RPY, 先绕x轴roll,再绕y轴pitch,最后绕z轴yaw) // Eigen::Vector3d ea(0.785398, -0, 0); Eigen::Vector3d ea(1.5708, 0.7854, 0.7854); //0 1 2 对应 z y x //3.1 欧拉角转换为旋转矩阵 Eigen::Matrix3d rotation_matrix_4; rotation_matrix_4 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout << "rotation matrix_4 =\n" << rotation_matrix_4 << endl; Eigen::Quaterniond my_q; my_q = Eigen::Quaterniond ( rotation_matrix_4 ); cout<<"quaternion = \n"<<my_q.coeffs() <<endl; // x y z w // cout << "Eigen::Vector3d::UnitZ() = \n" << Eigen::Vector3d::UnitZ() << endl; //0 0 1 // cout << "Eigen::Vector3d::UnitY() = \n" << Eigen::Vector3d::UnitY() << endl; //0 1 0 // cout << "Eigen::Vector3d::UnitX() = \n" << Eigen::Vector3d::UnitX() << endl; //1 0 0 /// 通过轴角方式求旋转矩阵 Eigen::AngleAxisd rotation_vector1(ea[0], Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd rotation_vector2(ea[1], Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rotation_vector3(ea[2], Eigen::Vector3d::UnitX()); /// compute RZ(alpha) 就是代入公式计算 Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity(); rotation_matrix1 = rotation_vector1.matrix(); // cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; /// compute RY(beta) Eigen::Matrix3d rotation_matrix2 = Eigen::Matrix3d::Identity(); rotation_matrix2 = rotation_vector2.matrix(); // cout << "rotation matrix2 =\n" << rotation_matrix2 << endl; /// compute RX(gamma) Eigen::Matrix3d rotation_matrix3 = Eigen::Matrix3d::Identity(); rotation_matrix3 = rotation_vector3.matrix(); // cout << "rotation matrix3 =\n" << rotation_matrix3 << endl; ///轴角直接相乘的结果 等于 三个旋转矩阵相乘的结果 即rotation_matrix_4 == rotation_all Eigen::Matrix3d rotation_all = rotation_matrix1 * rotation_matrix2 * rotation_matrix3; cout << "rotation_all =\n" << rotation_all << endl;