Eigen 四元數、歐拉角、旋轉矩陣、旋轉向量相互轉換

一、旋轉向量

1.0 初始化旋轉向量：旋轉角爲alpha，旋轉軸爲(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

1.1 旋轉向量轉旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.2 旋轉向量轉歐拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

1.3 旋轉向量轉四元數

Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
Quaterniond quaternion;
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

二、旋轉矩陣

2.0 初始化旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2.1 旋轉矩陣轉旋轉向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.2 旋轉向量轉歐拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

2.3 旋轉向量轉四元數

Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

三、歐拉角

3.0 初始化歐拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

3.1 歐拉角轉旋轉向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3.2 歐拉角轉旋轉矩陣

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.3 歐拉角轉四元數

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

四、四元數

4.0 初始化四元數

Eigen::Quaterniond quaternion(w,x,y,z);

4.1 四元數轉旋轉向量

Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.2 四元數轉旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

4.4 四元數轉歐拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);