一、概述
旋转变换的核心思想
旋转变换的五种表述
- 旋转矩阵；
- 欧式矩阵；
- 旋转向量；
- 欧拉角；
- 四元数；
旋转变换表述的演替
- 旋转矩阵和平移矩阵：有小尾巴累积
- 欧式矩阵：n+1维方阵要\((n+1)^2\)个自由度，太多了
- 旋转向量：这是什么呀这一堆数？！看不懂！
- 欧拉角：这会看懂了…等等，这转个90\(^\circ\)咋就膈屁了呢？！
- 四元数：爱咋转咋转…等等不对！咋1个\(R\)冒俩\(q\)呢？\(q\)咋还内讧了呢？
在Eigen库中它们四个大哥（欧式矩阵对不起，现在我们只考虑旋转）的转换关系
旋转矩阵
初始化旋转矩阵
Eigen::Matrix3d rotation_matrix; // 通过标准输入设备（标准输入流）键入赋值 rotation_matrix << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;旋转矩阵 \(\Longrightarrow\) 旋转向量
// 第一种：通过构造函数（传入一个旋转矩阵） Eigen::AngleAxisd rotation_vector(rotation_matrix); // 第二种：首先初始化，然后通过旋转矩阵直接赋值（重载了赋值运算符） Eigen::AngleAxisd rotation_vector; rotation_vector = rotation_matrix; // 第三种：首先初始化，然后from函数直接作用于this对象（rotation_vector） Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(rotation_matrix);旋转矩阵 \(\Longrightarrow\) 欧拉角
// (2, 1, 0)表示旋转顺序ZYX，数字越小表示优先级越高 Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0);旋转矩阵 \(\Longrightarrow\) 四元数
// 第一种：通过构造函数（传入一个旋转矩阵） Eigen::Quaterniond quaternion(rotation_matrix); // 第二种：首先初始化，然后通过旋转矩阵直接赋值（重载了赋值运算符） Eigen::Quaterniond quaternion; quaternion = rotation_matrix;
旋转向量
初始化旋转向量
// 通过构造函数 Eigen::AngleAxisd rotation_vector(alpha, Vector3d(x,y,z));旋转向量 \(\Longrightarrow\) 旋转矩阵
// 第一种方法：通过构造方法传入旋转向量 Eigen::Matrix3d rotation_matrix(rotation_vector); // 第二种方法：首先初始化，然后通过旋转向量直接赋值（重载了赋值运算符） Eigen::Matrix3d rotation_matrix; rotation_matrix = rotation_vector; // 第三种方法：通过matrix方法 Eigen::Matrix3d rotation_matrix = rotation_vector.matrix(); // 第四种方法：通过toRotationMatrix方法 Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();旋转向量 \(\Longrightarrow\) 欧拉角
// 不能直接转换，需要通过旋转矩阵搭桥 Eigen::Vector3d euler_angles = rotation_vector.matrix().eulerAngles(2, 1, 0);旋转向量 \(\Longrightarrow\) 四元数
// 第一种方法：通过构造函数传入旋转向量 Eigen::Quaterniond quaterniond(rotation_vector); // 第二种方法：首先初始化，然后用旋转向量赋值 Eigen::Quaterniond quaterniond; quaterniond = rotation_vector;
欧拉角
初始化欧拉角
Eigen::Vector3d euler_angles(yaw, pitch, roll);欧拉角 \(\Longrightarrow\) 旋转矩阵
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ())); // 先初始化旋转矩阵为单位矩阵，然后这三个旋转向量相乘得到旋转矩阵（运算符重载） Eigen::Matrix3d rotation_matrix; rotation_matrix = yawAngle * pitchAngle * rollAngle;欧拉角 \(\Longrightarrow\) 旋转向量
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitZ())); // 先初始化旋转向量，然后这三个旋转向量相乘得到旋转向量（运算符重载） Eigen::AngleAxisd rotation_vector; rotation_vector = yawAngle * pitchAngle * rollAngle;欧拉角 \(\Longrightarrow\) 四元数
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ())); // 先初始化四元数，然后这三个旋转向量相乘得到旋转向量（运算符重载） Eigen::Quaterniond quaterniond; quaterniond = yawAngle * pitchAngle * rollAngle;
四元数
初始化四元数
Eigen::Quaterniond quaterniond(w, x, y, z);四元数 \(\Longrightarrow\) 旋转矩阵
// 第一种方法：通过构造方法传入四元数 Eigen::Matrix3d rotation_matrix(quaterniond); // 第二种方法：首先初始化，然后通过四元数直接赋值（重载了赋值运算符） Eigen::Matrix3d rotation_matrix; rotation_matrix = quaterniond; // 第三种方法：通过matrix方法 Eigen::Matrix3d rotation_matrix = quaterniond.matrix(); // 第四种方法：通过toRotationMatrix方法 Eigen::Matrix3d rotation_matrix = quaterniond.toRotationMatrix();四元数 \(\Longrightarrow\) 旋转向量
// 第一种方法：通过构造函数传入一个四元数 Eigen::AngleAxisd rotation_vector(quaterniond); // 第二种方法：通过四元数直接赋值（运算符重载） Eigen::AngleAxisd rotation_vector; rotation_vector = quaterniond;四元数 \(\Longrightarrow\) 欧拉角
// 不能直接转换，需要靠旋转矩阵搭桥 Eigen::Vector3d euler_angles = quaterniond.matrix().eulerAngles(2, 1, 0);
在Eigen中的转换——总结篇
- 旋转矩阵到旋转向量的FRM()方法是fromRotationMatrix()；
- 四元数和旋转向量到旋转矩阵用的同一套体系，其中TRM()方法是toRotationMatrix()；
- 只有旋转矩阵才能直接转换为欧拉角，其EA()方法为eulerAngles()；
- 欧拉角转换成其他旋转表述形式用的同一套体系：RPY相乘。先初始化三个旋转角（RPY）的旋转向量，然后初始化所需旋转表述形式，最后这三个旋转向量相乘得到相应旋转表述形式（运算符重载）；
旋转表述的使用
旋转矩阵
Eigen::Vector3d v( 1,0,0 ); v_rotated = rotation_matrix * v;欧式矩阵
Eigen::Vector3d v( 1,0,0 ); Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 为欧式矩阵设置旋转矩阵 T.rotate(rotation_vector); // 为欧式矩阵设置平移矩阵 T.pretranslate(Eigen::Vector3d(1, 3, 4)); Eigen::Vector3d v_transformed = T * v;旋转向量
Eigen::Vector3d v( 1,0,0 ); Eigen::Vector3d v_rotated = rotation_vector * v;欧拉角
Eigen::Vector3d v( 1,0,0 ); Eigen::Vector3d euler_angles(M_PI / 4, M_PI / 4, M_PI / 4); // 通过上述转换：rotation_matrix !!! Eigen::Vector3d v_rotated = rotation_matrix * v;四元数
Eigen::Vector3d v( 1,0,0 ); Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector); // 注意数学上的表达式是：qvq^{-1} Eigen::Vector3d v_rotated = q * v;
二、详述
旋转矩阵
旋转矩阵的定义
\[\begin{aligned}&由旋转的本质方程：\beta_1^T\alpha_1=\beta_2^T\alpha_2，又由于\beta是标准正交基，所以\beta\beta^T = E；\\&所以两边同时乘上\beta_1，故而可得\alpha_1=\beta_1\beta_2^T\alpha_2，记旋转矩阵R=\beta_1\beta_2^T；\end{aligned}\]旋转矩阵各个参数的意义
旋转矩阵各个参数的计算
欧式矩阵
欧式矩阵的定义
\[T =\left[\begin{matrix}R&t\\\it{0}^T&1\end{matrix}\right]\]欧式矩阵各个参数的意义
欧式矩阵各个参数的计算
旋转向量
旋转向量的定义
\[\overrightarrow{n}与旋角\theta\]旋转向量各个参数的意义
旋转向量各个参数的计算
旋转轴\(\overrightarrow{n}\)的计算
旋转角\(\theta\)的计算
欧拉角
欧拉角的定义
欧拉角各个参数的意义
R：Roll，偏航角
P：Pitch，翻滚角
Y：Yaw，俯仰角
欧拉角各个参数的计算
四元数
四元数的定义
\[q=(s，\overrightarrow{v})^{T}=(s，x，y，z)^{T}=s+xi+yj+zk\]四元数各个参数的意义
实部\(s\)表示旋转程度：\(s=f(\theta)\)；
虚部\(\overrightarrow{v}\)表示旋转轴：\(\overrightarrow{v}=k\overrightarrow{n}\)；
四元数各个参数的计算（利用旋转向量）
实部\(s\)的计算
四元数 \(\Longrightarrow\) 旋转矩阵
\[\begin{aligned}R& = \overrightarrow{v}\overrightarrow{v}^{T}+s^2I+2s\overrightarrow{v}^{\wedge}+(\overrightarrow{v})^2\\\\\mathbf{tr}(R)&=4s^2-1\end{aligned}\]旋转矩阵 \(\Longrightarrow\) 旋转向量
\[\begin{aligned}\theta& = \arccos(\frac{\mathbf{tr}(R)-1}{2})=\arccos(2s^2-1)\\\\\theta& = 2\arccos{s}\\\\s& = \cos{\frac{\theta}{2}}\end{aligned}\]
虚部\(\overrightarrow{v}\)的计算
得到旋转轴
将四元数单位化