/**** 3. Euler angle ****/
cout << endl << "********** EulerAngle **********" << endl;
//3.0 Initialize the Euler angle (Z-Y-X, that is, RPY, first roll around the x-axis, then pitch around the y-axis, and finally yaw around the z-axis)
// Eigen::Vector3d ea(0.785398, -0, 0);
Eigen::Vector3d ea(1.5708, 0.7854, 0.7854); //0 1 2 Corresponding to z y x
//3.1 Euler angle conversion to rotation matrix
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
/// Find the rotation matrix by axis angle
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) is to substitute formula calculation
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;
///The result of direct multiplication of axis angles is equal to the result of multiplication of three rotation matrices, that is, rotation_matrix_4 == rotation_all
Eigen::Matrix3d rotation_all = rotation_matrix1 * rotation_matrix2 * rotation_matrix3;
cout << "rotation_all =\n" << rotation_all << endl;
