Eigen中的旋转操作
旋转矩阵
- 构造
Eigen::Matrix3d rotation_matrix;
rotation_matrix<< 0.707107, -0.707107, 0,
0.707107, 0.707107, 0,
0, 0, 1;
四元数
- 构造
//直接构造
Eigen::Quaterniond(1,0,0,0);
// 从旋转矩阵
Eigen::Matrix3d R;
R<< 0.707107, -0.707107, 0,
0.707107, 0.707107, 0,
0, 0, 1;
Eigen::Quaterniond q(R);
// 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度
Eigen::Quaterniond q(rotation_vector);
//从数组, 数组的顺序应该是[x y z w]
double array[4]= {
1,2,3,4};
Eigen::Quaterniond q1(array); //相当于直接构造 q1(4,1,2,3);
- 读取系数
// 1.
q.coeffs() // x, y, z, w 的顺序
q.vec() // x, y, z 虚数部分
q.x();
q.y();
q.z();
q.w();
- 其他常用操作
- 归一化
normalize()
:直接改原始的q,normalized() const
:返回一个归一化的q,原始不改变 - 返回旋转矩阵
toRotationMatrix
- 取模
norm()
- 模长的平方
squaredNorm()
- 取共轭
conjugate()
- 取逆
inverse()
- 点乘
dot
欧拉角
- 注意事项:
- 常用的一种欧拉角顺序 yaw-pitch-roll, 所对应的坐标轴:Z, Y, X
- 我们如果定义欧拉角:
Eigen::Vector3d euler_angle(X, Y, Z)
, euler_angle(0) : roll …
// 1. 可以从旋转矩阵得到欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 旋转顺序z,y,x; 得到欧拉角(yaw, pitch roll)
角轴
- 注意事项:
- 角轴的表示的轴必须是单位向量, 角是弧度制
- 角轴的表示遵守右手定则,大拇指朝上,四指的方向是正
- 角轴1(-M_PI/4, 0, 0, 1) 和 角轴2(M_PI/4, 0, 0, -1) 表示的是同一个旋转,绕z轴旋转-45度
- 构造
// 1. 从角轴
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度, 注意,角度制:弧度,轴:必须是单位的
// 2. 从四元数
Eigen::Quaterniond q1(1,1,1,1);
Eigen::AngleAxisd rotation_vector(q1); // q1可以不是归一化的,自动取q1的归一化之后的q取构造
// 3.从旋转矩阵
Eigen::Matrix3d R;
R<< 0.707107, -0.707107, 0,
0.707107, 0.707107, 0,
0, 0, 1;
Eigen::AngleAxisd rotation_vector(R);
- 其他常用操作
- 转到旋转矩阵
toRotationMatrix() const
- 取角度(弧度制)
angle()
- 取轴(单位向量)
axis()
自动单位化输出 - 相反的旋转
inverse() const
变换矩阵
- 构造
// 1. 可以从角轴和平移
Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
Eigen::Vector3d t(1,1,1);
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(n);
T.pretranslate(t);
// 2. 可以从四元数和平移
Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
Eigen::Quaterniond q(n);
Eigen::Vector3d t(1,1,1);
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(q);
T.pretranslate(t);
// 3. 可以从旋转矩阵和平移
Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
Eigen::Matrix3d R = n.matrix();
Eigen::Vector3d t(1,1,1);
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.pretranslate(t);
// 4. 除了创建好对象再传递 旋转 和平移之外,还可以以旋转创建,再传递平移,以下以旋转矩阵为例,其他两种一样可以
Eigen::Isometry3d T(R);
T.pretranslate(t);
- 常用操作
p = T*v
T内部重载了符号,虽然T是四维的, v和p都是3维的,但是相当于p = Rv+t的操作translation()
取平移部分rotation()
取旋转矩阵
各种旋转之间的转化
四元数和角轴
单位四元数: ( q 0 , q 1 , q 2 , q 3 ) (q_0, q_1, q_2, q_3) (q0,q1,q2,q3) 角轴: n θ n\theta nθ
- 四元数到角轴,得到轴是单位的
θ = 2 a r c cos q 0 \theta=2arc \cos q_0 θ=2arccosq0
n = [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n θ 2 n= [n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T/sin{\frac{\theta}{2}} n=[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
- 角轴到四元数轴, n = ( n x , n y , n z ) n= (n_x, n_y, n_z) n=(nx,ny,nz)是单位的,得到四元数是单位的
q = [ q 0 , q 1 , q 2 , q 3 ] T = [ cos θ 2 , n s i n θ 2 , ] T = [ cos θ 2 , n x s i n θ 2 , n y s i n θ 2 n z s i n θ 2 ] T q = [q_0, q_1, q_2, q_3]^T = [\cos{\frac{\theta}{2}}, \textbf{n}sin {\frac{\theta}{2}},]^T=[\cos{\frac{\theta}{2}}, n_x sin {\frac{\theta}{2}},n_y sin {\frac{\theta}{2}}n_z sin {\frac{\theta}{2}}]^T q=[q0,q1,q2,q3]T=[cos2θ,nsin2θ,]T=[cos2θ,nxsin2θ,nysin2θnzsin2θ]T
四元数和旋转矩阵
单位四元数: ( q 0 , q 1 , q 2 , q 3 ) (q_0, q_1, q_2, q_3) (q0,q1,q2,q3) 旋转矩阵R:
- 四元数到旋转矩阵
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R= \left[ \begin{matrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{matrix} \right] R=⎣⎡1−2q22−2q322q1<