本文学习来源主要基于高翔博士—slam14讲
目录
1.5变换矩阵(旋转矩阵加上平移量,所以并不是一种新的表示旋转的方法)
2.1 旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )
1、表示旋转的几个方法
1.1 旋转矩阵
它是一个行列式为 1 的正交矩阵
1.2旋转向量(或轴角,AxisAngle)
一个旋转轴为 n,角度为 θ 的旋转,它对应的旋转向 量为 θn。
1.3欧拉角
绕物体的 Z 轴旋转,得到偏航角 yaw;
绕旋转之后的 Y 轴旋转,得到俯仰角 pitch;
绕旋转之后的 X 轴旋转,得到滚转角 roll。
备注:只用于人机交互中;
不会在程序中直接使用欧拉角表达姿态,同样不会在滤波或优化中使用欧拉角表达旋
1.4四元数
i, j, k 为四元数的三个虚部
1.5变换矩阵(旋转矩阵加上平移量,所以并不是一种新的表示旋转的方法)
两次变换(旋转+平移=变换)如果按如下方式表达式会显得过于复
如果一次变换按如下表示:
我们把一个三维向量的末尾添加 1,变成了四维向量,称为齐次 坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面,使得整个关系变成 了线性关系。该式中,矩阵 T 称为变换矩阵(Transform Matrix)
而两次(或者多次变换)可以写成以下方式(看起来简介易懂)
2、各个表示旋转方式之间的转换
2.1 旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )
2.2旋转向量与四元数
2.3旋转矩阵与四元数
假设矩阵为 R = {mij}, i, j ∈ [1, 2, 3],其对 应的四元数 q 由下式给出:
3、代码实操
//基于高翔博士的代码,增加了一点注释
#include <iostream>
#include <cmath>
using namespace std;
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace Eigen;
// 本程序演示了 Eigen 几何模块的使用方法
int main(int argc, char **argv) {
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f Identity()表示3*3单位矩阵
Matrix3d rotation_matrix = Matrix3d::Identity();
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度
cout.precision(3);
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; //用matrix()转换成矩阵
// 也可以直接赋值//旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )变换
rotation_matrix = rotation_vector.toRotationMatrix();
// 用 AngleAxis 可以进行坐标变换
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角 Eigen::vector3d eulerAngles(Index a0, Index a1, Index a2),其中参数a0 a1 a2 用 0 1 2表示旋转轴,其中0表示X轴,1表示Y轴,2表示Z轴。
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序,即yaw-pitch-roll顺序 //此外,a0 表示首先选择的轴,a1 表示其次旋转的轴,a2 表示最后旋转的轴。输出结果的顺序与轴旋转的顺序相同。 Pi/4 = 0.785, 即沿着z轴旋转45度
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
// 欧氏变换矩阵使用 Eigen::Isometry
Isometry3d T = Isometry3d::Identity(); // 虽然称为3d,实质上是4*4的矩阵
T.rotate(rotation_vector); // 按照rotation_vector进行旋转 0.707 0.707 0
T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)
//T(0,3)=1; T(1,3)=3; T(2,3)=4;
cout << "Transform matrix = \n" << T.matrix() << endl;
// 用变换矩阵进行坐标变换做了什么变换:把(1,0,0)做了沿 Z 轴旋转 45 度和平移(1,3,4)
Vector3d v_transformed = T * v; // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;
// 对于仿射和射影变换,使用 Eigen::Affine3d 和 Eigen::Projective3d 即可,略
// 四元数
// 可以直接把AngleAxis赋值给四元数,反之亦然???coeffs是什么
Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
<< endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部
// 也可以把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
/四元数转换成旋转矩阵
//Eigen::Matrix3d qm = q.toRotationMatrix();
// 使用四元数旋转一个向量,使用重载的乘法即可
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
// 用常规向量乘法表示,则应该如下计算
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
return 0;
}
以下是终端输出的代码结果。