eigen之三維旋轉運動表達
阿新 • • 發佈：2021-06-26
目錄
簡介
用於表示三維剛體旋轉運動的方法主要有：
- 旋轉向量
- 旋轉矩陣
- 尤拉角
- 四元數
旋轉向量
#include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> int main(int argc, char** argv) { /**** 1. 旋轉向量 ****/ 尤拉角的實際範圍是: roll-[-pi:pi], pitch-[-pi/2:pi/2], yaw-[-pi:pi], 但是Eigen庫中函式的範圍是[0:pi], pitch-[-pi:pi], yaw-[-pi:pi]. std::cout << "\n ********** AngleAxis **********" << std::endl; Eigen::AngleAxisd rotation_vector1(M_PI/4, Eigen::Vector3d(0, 0, 1)); std::cout << "rotation vector1: " << "angle is: " << rotation_vector1.angle() * (180 / M_PI) << " axis is: " << rotation_vector1.axis().transpose() << std::endl; // 旋轉向量 -> 旋轉矩陣 Eigen::Matrix3d rotation_matrix1 = rotation_vector1.matrix(); //用matrix()轉換成旋轉矩陣 std::cout << "rotation matrix1 : \n" << rotation_matrix1 << std::endl; Eigen::Matrix3d rotation_matrix1_1 = rotation_vector1.toRotationMatrix(); // 由羅德里格公式進行轉換 std::cout << "rotation matrix1_1 : \n" << rotation_matrix1_1 << std::endl; // 旋轉向量 -> 尤拉角 Eigen::Vector3d euler_angle1 = rotation_vector1.matrix().eulerAngles(2, 1, 0); // 先轉為旋轉矩陣, 然後轉為尤拉角 std::cout << "euler angle: " << euler_angle1.transpose() << std::endl; // 旋轉向量 -> 四元數 Eigen::Quaterniond quaternion1(rotation_vector1); // 直接初始化 Eigen::Quaterniond quaternion1_1; quaternion1_1 = rotation_vector1; // 通過賦值 std::cout << "quaternion1: " << quaternion1.coeffs().transpose() << std::endl; // x y z w std::cout << "quaternion1_1: " << quaternion1_1.coeffs().transpose() << std::endl; return 0; }
結果:
********** AngleAxis ********** rotation vector1: angle is: 45 axis is: 0 0 1 rotation matrix1 : 0.707107 -0.707107 0 0.707107 0.707107 0 0 0 1 rotation matrix1_1 : 0.707107 -0.707107 0 0.707107 0.707107 0 0 0 1 euler angle: 0.785398 -0 0 quaternion1: 0 0 0.382683 0.92388 quaternion1_1: 0 0 0.382683 0.92388
旋轉矩陣
#include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> int main(int argc, char** argv) { /**** 2. 旋轉矩陣 *****/ std::cout << "\n ********** RotationMatrix **********" << std::endl; Eigen::Matrix3d rotation_matrix2; rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1; std::cout << "rotation matrix2 : \n" << rotation_matrix2 << std::endl; // 旋轉矩陣 -> 旋轉向量 Eigen::AngleAxisd rotation_vector2; rotation_vector2.fromRotationMatrix(rotation_matrix2); // fromRotationMatrix Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2); // 直接初始化 std::cout << "rotation vector2: " << "angle is: " << rotation_vector2.angle() * (180 / M_PI) << " axis is: " << rotation_vector2.axis().transpose() << std::endl; std::cout << "rotation vector2_1: " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector2_1.axis().transpose() << std::endl; // 旋轉矩陣 -> 尤拉角 Eigen::Vector3d euler_angle2 = rotation_matrix2.eulerAngles(2, 1, 0); // ZYX順序, 即先繞x軸roll,再繞y軸pitch,最後繞z軸yaw std::cout << "euler angle2: " << euler_angle2.transpose() << std::endl; // // row pitch yaw // 旋轉矩陣 -> 四元數 Eigen::Quaterniond quaternion2(rotation_matrix2); Eigen::Quaterniond quaternion2_1; quaternion2_1 = rotation_matrix2; std::cout << "quaternion2: " << quaternion2.coeffs().transpose() << std::endl; // x y z w std::cout << "quaternion2_1: " << quaternion2_1.coeffs().transpose() << std::endl; return 0; }
結果:
********** RotationMatrix **********
rotation matrix2 :
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
rotation vector2: angle is: 45 axis is: 0 0 1
rotation vector2_1: angle is: 45 axis is: 0 0 1
euler angle2: 0.785398 -0 0
quaternion2: 0 0 0.382684 0.92388
quaternion2_1: 0 0 0.382684 0.92388
注: 單位旋轉矩陣的旋轉向量為(0, 0, 0)
尤拉角
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char** argv)
{
/**** 3. 尤拉角 ****/
std::cout << "\n ********** EulerAngle **********" << std::endl;
Eigen::Vector3d euler_angle3(0.785398, -0, 0); // row pitch yaw
std::cout << "euler angle3: " << euler_angle3.transpose() << std::endl;
// 尤拉角 -> 旋轉矩陣
Eigen::Matrix3d rotation_matrix3;
rotation_matrix3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
std::cout << "rotation matrix3 : \n" << rotation_matrix3 << std::endl;
// 尤拉角 -> 旋轉向量
Eigen::AngleAxisd rotation_vector3;
rotation_vector3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
std::cout << "rotation vector3: " << "angle is: " << rotation_vector3.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector3.axis().transpose() << std::endl;
// 尤拉角 -> 四元數
Eigen::Quaterniond quaternion3;
quaternion3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX());
std::cout << "quaternion2: " << quaternion3.coeffs().transpose() << std::endl; // x y z w
return 0;
}
結果:
********** EulerAngle **********
euler angle3: 0.785398 0 0
rotation matrix3 :
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
rotation vector3: angle is: 45 axis is: 0 0 1
quaternion2: 0 0 0.382683 0.92388
注: 尤拉角轉為旋轉向量的順序
四元數
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char** argv)
{
/**** 4.四元數 ****/
std::cout << "\n ********** Quaternion **********" << std::endl;
Eigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683); // w, x, y, z
// 四元數 -> 旋轉矩陣
Eigen::Matrix3d rotation_matrix4;
rotation_matrix4 = quaternion4.matrix();
Eigen::Matrix3d rotation_matrix4_1;
rotation_matrix4_1 = quaternion4.toRotationMatrix();
std::cout << "rotation matrix4 : \n" << rotation_matrix4 << std::endl;
std::cout << "rotation matrix4_1 : \n" << rotation_matrix4_1 << std::endl;
// 四元數 -> 旋轉向量
Eigen::AngleAxisd rotation_vector4(quaternion4);
Eigen::AngleAxisd rotation_vector4_1;
rotation_vector4_1 = quaternion4;
std::cout << "rotation vector4: " << "angle is: " << rotation_vector4.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector4.axis().transpose() << std::endl;
std::cout << "rotation vector4_1: " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector4_1.axis().transpose() << std::endl;
// 四元數 -> 尤拉角
Eigen::Vector3d euler_angle4 = quaternion4.matrix().eulerAngles(2, 1, 0);
std::cout << "euler angle4: " << euler_angle4.transpose() << std::endl;
return 0;
}
結果
********** Quaternion **********
rotation matrix4 :
0.707107 -0.707106 0
0.707106 0.707107 0
0 0 1
rotation matrix4_1 :
0.707107 -0.707106 0
0.707106 0.707107 0
0 0 1
rotation vector4: angle is: 44.9999 axis is: 0 0 1
rotation vector4_1: angle is: 44.9999 axis is: 0 0 1
euler angle4: 0.785397 -0 0
注: 四元數到尤拉角有時會錯.
筆記
二維旋轉公式
三維旋轉公式
尤拉角
在Eigen庫中, 其他運動表達轉尤拉角都是通過rotation.eulerAngles(2, 1, 0)函式, 即先轉為旋轉矩陣然後再轉為尤拉角. 這個函式有個問題就是出來尤拉角的範圍不對.
- 尤拉角的實際範圍是: roll-[-pi:pi], pitch-[-pi/2:pi/2], yaw-[-pi:pi]
- Eigen庫中函式的範圍是:roll-[0:pi], pitch-[-pi:pi], yaw-[-pi:pi].
所以推薦使用ros中的tf函式得到尤拉角，而不是eigen或者pcl中的函式