第3讲：三维空间刚体运动

三维空间中刚体运动的描述方式：旋转矩阵、变换矩阵、四元数和欧拉角

3.1 旋转矩阵

3.1.1 点和向量，坐标系

三维空间中，给定线性空间基(e1,e2,e3)(\mathbf{e}_{1}, \mathbf{e}_{2}, \mathbf{e}_{3})(e1​,e2​,e3​)，则向量a,b∈R3\mathbf{a}, \mathbf{b} \in \R^{3}a,b∈R3，

a=[e1e2e3][a1a2a3]=a1e1+a2e2+a3e3(3-1)\mathbf{a} = \begin{bmatrix} \mathbf{e}_{1} & \mathbf{e}_{2} & \mathbf{e}_{3} \end{bmatrix} \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = a_{1}\mathbf{e}_{1} + a_{2}\mathbf{e}_{2} + a_{3}\mathbf{e}_{3} \tag {3-1}a=[e1​​e2​​e3​​]⎣⎡​a1​a2​a3​​⎦⎤​=a1​e1​+a2​e2​+a3​e3​(3-1)

内积：

a⋅b=aTb=∑i=13aibi=∣a∣∣b∣cos⁡⟨a,b⟩(3-2)\mathbf{a} \cdot \mathbf{b} = \mathbf{a}^{\text{T}} \mathbf{b} = \sum_{i = 1}^{3} a_{i} b_{i} = |\mathbf{a}| |\mathbf{b}| \cos \langle \mathbf{a}, \mathbf{b} \rangle \tag {3-2}a⋅b=aTb=i=1∑3​ai​bi​=∣a∣∣b∣cos⟨a,b⟩(3-2)

内积描述向量间的投影关系。外积为：

a×b=[ijka1a2a3b1b2b3]=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10]b≜a∧b(3-3)\mathbf{a} \times \mathbf{b} = \begin{bmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \\ \end{bmatrix} = \begin{bmatrix} a_{2} b_{3} - a_{3} b_{2} \\ a_{3} b_{1} - a_{1} b_{3} \\ a_{1} b_{2} - a_{2} b_{1} \\ \end{bmatrix} = \begin{bmatrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \\ \end{bmatrix} \mathbf{b} \triangleq \mathbf{a}^{\land} \mathbf{b} \tag {3-3}a×b=⎣⎡​ia1​b1​​ja2​b2​​ka3​b3​​⎦⎤​=⎣⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦⎤​=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​b≜a∧b(3-3)

外积的方向垂直于这两个向量，大小为∣a∣∣b∣⟨a,b⟩|\mathbf{a}| |\mathbf{b}|\langle \mathbf{a}, \mathbf{b} \rangle∣a∣∣b∣⟨a,b⟩，表示两个向量张成的四边形的有向面积。符号∧\land∧表示反对称符号，将a\mathbf{a}a改写成反对称矩阵（skew-symmetric）形式，其作用为将外积a×b\mathbf{a} \times \mathbf{b}a×b写成矩阵与向量的乘积a∧b\mathbf{a}^{\land} \mathbf{b}a∧b，从而将外积变成线性运算。外积只对三维向量存在定义，外积可以表示向量的旋转。

在右手法则下，用右手的四指从a\mathbf{a}a转向b\mathbf{b}b，其拇指方向就是旋转向量的方向，即a×b\mathbf{a} \times \mathbf{b}a×b的方向，其大小由 a\mathbf{a}a和b\mathbf{b}b的夹角决定。该方式构造了从a\mathbf{a}a到b\mathbf{b}b的旋转向量，该向量同样位于三维空间中，在此坐标系下，可以用三个实数描述。

3.1.2 坐标系间的欧氏变换

两个坐标系间的变换关系：旋转、平移。机器人在运动过程中，通常设定一个惯性坐标系（世界坐标系），认为其固定不动，如图中xW,yW,zWx_{W}, y_{W}, z_{W}xW​,yW​,zW​；相机或机器人则是一个移动坐标系，如xC,yC,zCx_{C}, y_{C}, z_{C}xC​,yC​,zC​。相机视野中某个向量p\mathbf{p}p，其坐标为pc\mathbf{p}_{c}pc​，在世界坐标系中，其坐标为pw\mathbf{p}_{w}pw​。

刚体运动：同一个向量在各个坐标系下的长度和夹角不会改变，这种变换称为欧氏变换。

旋转：假设一组单位正交基(e1,e2,e3)(\mathbf{e}_{1}, \mathbf{e}_{2}, \mathbf{e}_{3})(e1​,e2​,e3​)经一次旋转，变成(e1′,e2′,e3′)(\mathbf{e}_{1}^{\prime}, \mathbf{e}_{2}^{\prime}, \mathbf{e}_{3}^{\prime})(e1′​,e2′​,e3′​)，对于同一个向量a\mathbf{a}a（该向量并没有随坐标系旋转而运动），其在两个坐标系下的坐标分别为[a1,a2,a3]T[ a_{1}, a_{2}, a_{3} ]^{\text{T}}[a1​,a2​,a3​]T和[a1′,a2′,a3′]T[ a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime} ]^{\text{T}}[a1′​,a2′​,a3′​]T，则：

[e1e2e3][a1a2a3]=[e1′e2′e3′][a1′a2′a3′](3-4)\begin{bmatrix} \mathbf{e}_{1} & \mathbf{e}_{2} & \mathbf{e}_{3} \end{bmatrix} \begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\prime} \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} \tag {3-4}[e1​​e2​​e3​​]⎣⎡​a1​a2​a3​​⎦⎤​=[e1′​​e2′​​e3′​​]⎣⎡​a1′​a2′​a3′​​⎦⎤​(3-4)

由单位正交基性质可知：

[a1a2a3]=[e1Te2Te3T][e1′e2′e3′][a1′a2′a3′]=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′][a1′a2′a3′]≜Ra′(3-5)\begin{bmatrix} a_{1} \\ a_{2} \\ a_{3} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\text{T}} \\ \mathbf{e}_{2}^{\text{T}} \\ \mathbf{e}_{3}^{\text{T}} \end{bmatrix} \begin{bmatrix} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\prime} \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} = \begin{bmatrix} \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{1}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{2}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{1}^{\prime} & \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{2}^{\prime} & \mathbf{e}_{3}^{\text{T}} \mathbf{e}_{3}^{\prime} \\ \end{bmatrix} \begin{bmatrix} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{bmatrix} \triangleq \mathbf{R} \mathbf{a}^{\prime} \tag {3-5}⎣⎡​a1​a2​a3​​⎦⎤​=⎣⎡​e1T​e2T​e3T​​⎦⎤​[e1′​​e2′​​e3′​​]⎣⎡​a1′​a2′​a3′​​⎦⎤​=⎣⎡​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​⎦⎤​⎣⎡​a1′​a2′​a3′​​⎦⎤​≜Ra′(3-5)

矩阵R\mathbf{R}R由两组基之间的内积组成，表示旋转前后同一个向量的坐标变换关系，即R\mathbf{R}R表示旋转，又称为旋转矩阵（rotation matrix）。旋转矩阵是一个行列式为1的正交矩阵；反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，旋转矩阵的集合定义为：

SO(n)={R∈Rn×n∣RRT=I,det⁡(R)=1}(3-6)SO(n) = \{ \mathbf{R} \in \R^{n \times n} | \mathbf{R} \mathbf{R}^{\text{T}} = \mathbf{I}, \det(\mathbf{R}) = 1 \} \tag {3-6}SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(3-6)

SO(n)SO(n)SO(n)表示特殊正交群（special orthogonal group），SO(3)SO(3)SO(3)表示三维空间中的旋转。旋转矩阵可以描述相机的旋转。

旋转矩阵为正交阵，其逆（即转置）表示方向相反的旋转：

a′=R−1a=RTa(3-7)\mathbf{a}^{\prime} = \mathbf{R}^{-1} \mathbf{a} = \mathbf{R}^{\text{T}} \mathbf{a} \tag {3-7}a′=R−1a=RTa(3-7)

假设世界坐标系中的向量a\mathbf{a}a，经过旋转（R\mathbf{R}R）和平移t\mathbf{t}t后，得到a′\mathbf{a}^{\prime}a′，则：

a′=Ra+t(3-8)\mathbf{a}^{\prime} = \mathbf{R} \mathbf{a} + \mathbf{t} \tag {3-8}a′=Ra+t(3-8)

其中，t\mathbf{t}t为平移向量。旋转矩阵R\mathbf{R}R和平移向量t\mathbf{t}t共同完整地描述一个欧氏空间的坐标变换关系。

3.1.3 变换矩阵与齐次坐标

引入齐次坐标和变换矩阵重写方程（3-8）：

[a′1]=[Rt0T1][a1]≜T[a1](3-9)\begin{bmatrix} \mathbf{a}^{\prime} \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \begin{bmatrix} \mathbf{a} \\ 1 \end{bmatrix} \triangleq \mathbf{T} \begin{bmatrix} \mathbf{a} \\ 1 \end{bmatrix} \tag {3-9}[a′1​]=[R0T​t1​][a1​]≜T[a1​](3-9)

在三维向量的末尾添加1，组成四维向量，称为齐次坐标。对于齐次向量，可以将旋转和平移包含在一个矩阵里，其中矩阵T\mathbf{T}T称为变换矩阵（transform matrix）。用a~\tilde{\mathbf{a}}a~表示a\mathbf{a}a的齐次坐标。

在射影几何中，通过添加最后一维，用四个实数描述一个三维向量。在齐次坐标中，点x\mathbf{x}x的每个分量同乘一个非零常数kkk后，仍然表示同一个点。因此，点的坐标值并是唯一，如[1,1,1,1]T[1, 1, 1, 1]^{\text{T}}[1,1,1,1]T和[2,2,2,2]T[2, 2, 2, 2]^{\text{T}}[2,2,2,2]T表示同一个点。因此，当最后一项不为零时，将所有坐标除以最后一项，强制最后一项为 1，从而得到一个点唯一的坐标表示（即转换成非齐次坐标）：

x~=[x,y,z,w]T=[x/w,y/w,z/w,1]T(3-10)\tilde{\mathbf{x}} = [x, y, z, w]^{\text{T}} = [x/w, y/w, z/w, 1]^{\text{T}} \tag {3-10}x~=[x,y,z,w]T=[x/w,y/w,z/w,1]T(3-10)

多次欧氏变换可表示为：

b~=T1a~,c~=T2b~⇒c~=T2T1a~(3-11)\tilde{\mathbf{b}} = \mathbf{T}_{1} \tilde{\mathbf{a}}, \tilde{\mathbf{c}} = \mathbf{T}_{2} \tilde{\mathbf{b}} \Rightarrow \tilde{\mathbf{c}} = \mathbf{T}_{2} \mathbf{T}_{1} \tilde{\mathbf{a}} \tag {3-11}b~=T1​a~,c~=T2​b~⇒c~=T2​T1​a~(3-11)

变换矩阵T\mathbf{T}T的结构：左上角为旋转矩阵、右侧为平移向量、左下角为零向量、右下角为1，这种矩阵又称为特殊欧氏群（special euclidean group）：

SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}(3-12)SE(3) = \left\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \in \R^{4 \times 4} | \mathbf{R} \in SO(3), \mathbf{t} \in \R^{3} \right\} \tag {3-12}SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}(3-12)

变换矩阵T\mathbf{T}T的逆表示反向变换：

T−1=[RT−RTt0T1](3-13)\mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^{\text{T}} & - \mathbf{R}^{\text{T}} \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-13}T−1=[RT0T​−RTt1​](3-13)

在不引起歧义的情况下，齐次坐标与普通坐标符号一般不做区分，默认为符合所用运算法则的那一种。

3.2 实践：Eigen

// eigen_matrix.cpp
#include <iostream>
#include <ctime>using namespace std;// eigen
#include <Eigen/Core>
// 稠密矩阵代数运算（逆，特征值等）
#include <Eigen/Dense>#define MATRIX_SIZE 100/******************************
* 本程序演示了 Eigen 基本类型的使用
******************************/int main(int argc, char const *argv[])
{/* code */// Eigen以矩阵为基本数据单元。Matrix是一个模板类。前三个参数为：数据类型，行，列Eigen::Matrix<float, 2, 3> matrix_23;// Eigen通过`typedef`定义了许多内置类型，其底层仍为｀Eigen::Matrix｀// 例如`Vector3d`实质上是`Eigen::Matrix<double, 3, 1>`Eigen::Vector3d v_3d;Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();    // 初始化为零// 使用动态尺寸矩阵时，可以不指定矩阵大小Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;Eigen::MatrixXd matrix_x;// 矩阵操作// 输入数据matrix_23 << 1, 2, 3, 4, 5, 6;cout << matrix_23 << endl;// 用()访问矩阵中的元素for (int i = 0; i < 1; i++){for (int j = 0; j < 2; j ++){cout << matrix_23(i, j) << endl;}}v_3d << 3, 2, 1;// 矩阵和向量相乘（实际上仍是矩阵和矩阵）// 但是不能混合数据类型不同的矩阵，需要显式转换Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << result << endl;// 矩阵运算matrix_33 = Eigen::Matrix3d::Random();cout << matrix_33 << endl;// 转置cout << matrix_33.transpose() << endl;// 各元素和cout << matrix_33.sum() << endl;// 迹cout << matrix_33.trace() << endl;// 标量相乘cout << 10 * matrix_33 << endl;// 逆cout << matrix_33.inverse() << endl;// 行列式cout << matrix_33.determinant() << endl;// 特征值// 实对称矩阵可以保证成功对角化Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose() * matrix_33 );cout << "eigen values = " << eigen_solver.eigenvalues() << endl;cout << "eigen vectors = " << eigen_solver.eigenvectors() << endl;// 解方程// 求解`matrix_NN * x = v_Nd`// 直接求逆运算量大Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;v_Nd = Eigen::VectorXd::Random(MATRIX_SIZE);// 计时clock_t time_stt = clock();// 直接求逆Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;// 通常用矩阵分解求逆，例如`QR`分解，速度会快很多time_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);cout << "time use in Qr compsition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;return 0;
}

# CMakeLists.txt# 声明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )# 声明一个 cmake 工程
project( Eigen )# 添加头文件
include_directories( "/usr/include/eigen3" )# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable( eigen_matrix eigen_matrix.cpp )# 设置编译模式
set( CMAKE_BUILD_TYPE "Debug" )

3.3 旋转向量和欧拉角

3.3.1 旋转向量

矩阵表示方式的缺点：

1. SO(3)SO(3)SO(3)旋转矩阵有九个量，但一次旋转只有三个自由度，因此这种表达方式是冗余的。同理，变换矩阵用十六个量表达了六自由度变换；

2. 旋转矩阵自带约束：必须为正交矩阵，且行列式为1。变换矩阵也是如此。约束会使估计、优化旋转矩阵（变换矩阵）变得困难。

任意旋转都可以用一个旋转轴和一个旋转角表示，因此旋转可以用一个向量表示旋转，其方向与旋转轴一致、长度等于旋转角，这种向量称为旋转向量（rotation vector）（轴角，axis-angle）。同理，变换可以用一个旋转向量和一个平移向量表示，此时，维数恰好为6。

旋转向量和旋转矩阵之间的转换：假设有一个旋转轴为n\mathbf{n}n、角度为θ\thetaθ的旋转，其对应的旋转向量为θn\theta \mathbf{n}θn，由罗德里格斯公式（Rodrigues’s formula）：

R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧(3-14)\mathbf{R} = \cos \theta \mathbf{I} + (1 - \cos \theta) \mathbf{n} \mathbf{n}^{\text{T}} + \sin \theta \mathbf{n}^{\land} \tag {3-14}R=cosθI+(1−cosθ)nnT+sinθn∧(3-14)

其中，∧\land∧表示向量到反对称的转换符。旋转矩阵到旋转向量的转换：

tr(R)=cos⁡θtr(I)+(1−cos⁡θ)tr(nnT)+sin⁡θtr(n∧)=1+2cos⁡θ\begin{aligned} \text{tr}(\mathbf{R}) & = \cos \theta \text{tr}(\mathbf{I}) + (1 - \cos \theta) \text{tr}(\mathbf{n} \mathbf{n}^{\text{T}}) + \sin \theta \text{tr}(\mathbf{n}^{\land}) \\ & = 1 + 2 \cos \theta \end{aligned}tr(R)​=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n∧)=1+2cosθ​

因此，转角θ\thetaθ为：

θ=arccos⁡(tr(R)−12)(3-16)\theta = \arccos(\frac{\text{tr}(\mathbf{R}) - 1}{2}) \tag {3-16}θ=arccos(2tr(R)−1​)(3-16)

由于转轴向量n\mathbf{n}n旋转不变，因此

Rn=n\mathbf{R} \mathbf{n} = \mathbf{n}Rn=n

转轴n\mathbf{n}n是矩阵R\mathbf{R}R特征值1对应的特征向量。

3.3.2 欧拉角

**欧拉角（Euler angle）**用三个分离的转角把一个旋转分解成三次绕不同轴的旋转，提供了一种直观方式描述旋转。

根据不同的分解方式，欧拉角存在不同的定义方法。例如先绕X轴旋转，再绕Y轴，最后绕Z轴，就得到了一个XYZ轴的旋转。同理，可以定义ZYZ、ZYX等旋转方式。此外，还需区分每次旋转是绕固定轴，还是绕旋转后的轴。

欧拉角常用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度描述一个旋转，其等价于ZYX轴旋转。假设一个刚体的前方为X轴，右侧为Y轴，上方为Z轴，ZYX转角把旋转分解成：

1. 绕物体的Z轴旋转，得到偏航角（yaw）；

2. 绕旋转之后的Y轴旋转，得到俯仰角（pitch）；

3. 绕旋转之后的X轴旋转，得到滚转角（roll）。

此时，使用三维向量[r,p,y]T[r, p, y]^{\text{T}}[r,p,y]T描述任意旋转。欧拉角的缺点是万向锁问题（gimbal lock）：当俯仰角为±90∘\pm90^{\circ}±90∘时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（三次旋转变成两次旋转），称为奇异性问题。理论上，只要用三个实数表示三维旋转时，都会碰到奇异性问题。因此，欧拉角不适于插值和迭代，只能用于人机交互。

3.4 四元数

3.4.1 四元数的定义

三维旋转是一个三维流形，若要无奇异性地表示，三个量不够，因此，三维向量表示旋转的无奇异性方式不存在。

四元数（quaternion）：Hamilton提出的一种扩展复数，紧凑且无奇异性。一个四元数q\mathbf{q}q有一个实部和三个虚部，通常实部在前（也有实部在后的）：

q=q0+q1i+q2j+q3k(3-17)\mathbf{q} = q_{0} + q_{1} i + q_{2} j + q_{3} k \tag {3-17}q=q0​+q1​i+q2​j+q3​k(3-17)

其中，i,j,ki, j, ki,j,k为四元数的三个虚部，三个虚部满足：

{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ij=−j(3-18)\begin{cases} i^{2} = j^{2} = k^{2} = -1 \\ ij = k, ji = -k \\ jk = i, kj = -i \\ ki = j, ij = -j \end{cases} \tag {3-18}⎩⎪⎪⎪⎨⎪⎪⎪⎧​i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ij=−j​(3-18)

四元数也用一个标量和一个向量表示：

q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3\mathbf{q} = [s, \mathbf{v}], \ s = q_{0} \in \R, \mathbf{v} = [q_{1}, q_{2}, q_{3}]^{\text{T}} \in \R^{3}q=[s,v], s=q0​∈R,v=[q1​,q2​,q3​]T∈R3

其中，sss为四元数实部，而v\mathbf{v}v为虚部。如果一个四元数虚部为000，称之为实四元数；反之，若其实部为000，称之为虚四元数。

单位四元数可以表示三维空间中任意旋转，假设一个旋转绕单位向量n=[nx,ny,nz]T\mathbf{n} = [n_{x}, n_{y}, n_{z}]^{\text{T}}n=[nx​,ny​,nz​]T旋转θ\thetaθ，则该旋转的四元数为：

q=[cos⁡θ2,nxsin⁡θ2,nysin⁡θ2,nzsin⁡θ2]T(3-19)\mathbf{q} = \left[ \cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2} \right]^{\text{T}} \tag {3-19}q=[cos2θ​,nx​sin2θ​,ny​sin2θ​,nz​sin2θ​]T(3-19)

单位四元数对应旋转轴与旋转角分别为：

{θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]Tsin⁡θ2(3-20)\begin{cases} \theta = 2 \arccos q_{0} \\ [n_{x}, n_{y}, n_{z}]^{\text{T}} = \frac{[q_{1}, q_{2}, q_{3}]^{\text{T}}}{\sin \frac{\theta}{2}} \end{cases} \tag {3-20}{θ=2arccosq0​[nx​,ny​,nz​]T=sin2θ​[q1​,q2​,q3​]T​​(3-20)

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。当θ=0\theta = 0θ=0时，得到一个没有旋转的实四元数：

q0=[±1,0,0,0]T(3-21)\mathbf{q}_{0} = \left[ \pm 1, 0, 0, 0 \right]^{\text{T}} \tag {3-21}q0​=[±1,0,0,0]T(3-21)

3.4.2 四元数的运算

四元数包括四则运算、数乘、求逆、共轭等。

给定两个四元数

qa=[sa,va]=sa+xai+yaj+zak\mathbf{q}_{a} = [s_{a}, \mathbf{v}_{a}] = s_{a} + x_{a} i + y_{a} j + z_{a} kqa​=[sa​,va​]=sa​+xa​i+ya​j+za​k

qb=[sb,vb]=sb+xbi+ybj+zbk\mathbf{q}_{b} = [s_{b}, \mathbf{v}_{b}] = s_{b} + x_{b} i + y_{b} j + z_{b} kqb​=[sb​,vb​]=sb​+xb​i+yb​j+zb​k

1. 加法和减法

qa±qb=[sa±sb,va±vb](3-22)\mathbf{q}_{a} \pm \mathbf{q}_{b} = [s_{a} \pm s_{b}, \mathbf{v}_{a} \pm \mathbf{v}_{b}] \tag {3-22}qa​±qb​=[sa​±sb​,va​±vb​](3-22)

1. 乘法：qa\mathbf{q}_{a}qa​的各项与qb\mathbf{q}_{b}qb​的各项分别相乘，然后相加，虚部遵从方程（3-18）

qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k=[sasb−vaTvb,sava+sbvb+va×vb](3-24)\begin{aligned} \mathbf{q}_{a} \mathbf{q}_{b} & = s_{a} s_{b} - x_{a} x_{b} - y_{a} y_{b} - z_{a} z_{b} \\ & + (s_{a} x_{b} + x_{a} s_{b} + y_{a} z_{b} - z_{a} y_{b}) i \\ & + (s_{a} y_{b} - x_{a} z_{b} + y_{a} s_{b} + z_{a} x_{b}) j \\ & + (s_{a} z_{b} + x_{a} y_{b} - y_{a} x_{b} + z_{a} s_{b}) k \\ & = \left[ s_{a} s_{b} - \mathbf{v}_{a}^{\text{T}} \mathbf{v}_{b}, s_{a} \mathbf{v}_{a} + s_{b} \mathbf{v}_{b} + \mathbf{v}_{a} \times \mathbf{v}_{b} \right] \end{aligned} \tag {3-24}qa​qb​​=sa​sb​−xa​xb​−ya​yb​−za​zb​+(sa​xb​+xa​sb​+ya​zb​−za​yb​)i+(sa​yb​−xa​zb​+ya​sb​+za​xb​)j+(sa​zb​+xa​yb​−ya​xb​+za​sb​)k=[sa​sb​−vaT​vb​,sa​va​+sb​vb​+va​×vb​]​(3-24)

四元数乘法通常不可交换，除非va\mathbf{v}_{a}va​和vb\mathbf{v}_{b}vb​在R3\R^{3}R3中共线，此时外积项为零。

1. 共轭：四元数共轭是把虚部取成相反数

q∗=[s,−v]=s−xi−yj−zk(3-25)\mathbf{q}^{\ast} = [s, - \mathbf{v}] = s - x i - y j - z k \tag {3-25}q∗=[s,−v]=s−xi−yj−zk(3-25)

四元数共轭与自身相乘，得到一个实四元数，其实部为模长的平方：

q∗q=qq∗=[s2−vTv,0](3-26)\mathbf{q}^{\ast} \mathbf{q} = \mathbf{q} \mathbf{q}^{\ast} = \left[ s^{2} - \mathbf{v}^{\text{T}} \mathbf{v}, \mathbf{0} \right] \tag {3-26}q∗q=qq∗=[s2−vTv,0](3-26)

1. 模长

∥q∥=s2+x2+y2+z2(3-27)\| \mathbf{q} \| = \sqrt{ s^{2} + x^{2} + y^{2} + z^{2} } \tag {3-27}∥q∥=s2+x2+y2+z2​(3-27)

两个四元数乘积的模即为模的乘积，单位四元数相乘后仍是单位四元数。

∥qaqb∥=∥qa∥∥qb∥(3-28)\| \mathbf{q}_{a} \mathbf{q}_{b} \| = \| \mathbf{q}_{a} \| \| \mathbf{q}_{b} \| \tag {3-28}∥qa​qb​∥=∥qa​∥∥qb​∥(3-28)

1. 逆

q−1=q∗∥q∥(3-29)\mathbf{q}^{-1} = \frac{\mathbf{q}^{\ast}}{\| \mathbf{q} \|} \tag {3-29}q−1=∥q∥q∗​(3-29)

则四元数与其逆的乘积为实四元数1\mathbf{1}1：

q−1q=qq−1=1(3-30)\mathbf{q}^{-1} \mathbf{q} = \mathbf{q} \mathbf{q}^{-1} = \mathbf{1} \tag {3-30}q−1q=qq−1=1(3-30)

单位四元数的逆和共轭相同。乘积的逆满足

(qaqb)−1=qb−1qa−1(3-31)(\mathbf{q}_{a} \mathbf{q}_{b})^{-1} = \mathbf{q}_{b}^{-1} \mathbf{q}_{a}^{-1} \tag {3-31}(qa​qb​)−1=qb−1​qa−1​(3-31)

1. 数乘与点乘

四元数与标量相乘

kq=[ks,kv](3-32)k \mathbf{q} =[ks, k \mathbf{v}] \tag {3-32}kq=[ks,kv](3-32)

点乘：两个四元数对应位置元素分别相乘

qa⋅qb=sasb+xaxbi+yaybj+zazbk(3-33)\mathbf{q}_{a} \cdot \mathbf{q}_{b} = s_{a} s_{b} + x_{a} x_{b} i + y_{a} y_{b} j + z_{a} z_{b} k \tag {3-33}qa​⋅qb​=sa​sb​+xa​xb​i+ya​yb​j+za​zb​k(3-33)

3.4.3 用四元数表示旋转

给定三维空间中点p=[x,y,z]∈R3\mathbf{p} = [x, y, z] \in \R^{3}p=[x,y,z]∈R3和轴角n,θ\mathbf{n}, \thetan,θ指定的旋转，点p\mathbf{p}p经旋转后记为p′\mathbf{p}^{\prime}p′，则点的旋转四元数表示为：

1. 将点p\mathbf{p}p表示为虚：

p=[0,x,y,z]=[0,v]\mathbf{p} = [0, x, y, z] = [0, \mathbf{v}]p=[0,x,y,z]=[0,v]

1. 由方程（3-19）旋转的四元数q\mathbf{q}q表示为：

q=[cos⁡θ2,nsin⁡θ2]\mathbf{q} = [\cos \frac{\theta}{2}, \mathbf{n} \sin \frac{\theta}{2}]q=[cos2θ​,nsin2θ​]

则旋转后点p′\mathbf{p}^{\prime}p′的表示为：

p′=qpq−1(3-34)\mathbf{p}^{\prime} = \mathbf{q} \mathbf{p} \mathbf{q}^{-1} \tag {3-34}p′=qpq−1(3-34)

3.4.4 四元数到旋转矩阵的转换

四元数q=q0+q1i+q2j+q3k\mathbf{q} = q_{0} + q_{1} i + q_{2} j + q_{3} kq=q0​+q1​i+q2​j+q3​k对应的旋转矩阵R\mathbf{R}R为：

R=[1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22](3-35)\mathbf{R} = \begin{bmatrix} 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} \end{bmatrix} \tag {3-35}R=⎣⎡​1−2q22​−2q32​2q1​q2​−2q0​q3​2q1​q3​+2q0​q2​​2q1​q2​+2q0​q3​1−2q12​−2q32​2q2​q3​−2q0​q1​​2q1​q3​−2q0​q2​2q2​q3​+2q0​q1​1−2q12​−2q22​​⎦⎤​(3-35)

反之，旋转矩阵R=[mij],i,j∈{1,2,3}\mathbf{R} = [ m_{ij} ], i, j \in \{1, 2, 3\}R=[mij​],i,j∈{1,2,3}对应的四元数q\mathbf{q}q为：

q0=tr(R)+12,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0(3-36)q_{0} = \frac{\sqrt{\text{tr}(\mathbf{R}) + 1}}{2}, q_{1} = \frac{m_{23} - m{32}}{4 q_{0}}, q_{2} = \frac{m_{31} - m{13}}{4 q_{0}}, q_{3} = \frac{m_{12} - m{21}}{4 q_{0}} \tag {3-36}q0​=2tr(R)+1​​,q1​=4q0​m23​−m32​,q2​=4q0​m31​−m13​,q3​=4q0​m12​−m21​(3-36)

3.5 相似、仿射、射影变换

欧氏变换保持向量的长度和夹角不变，相当于将一个刚体进行了移动或旋转，不改变其自身的样子。

1. 相似变换

相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为：

TS=[sRt0T1](3-37)\mathbf{T}_{S} = \begin{bmatrix} s \mathbf{R} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-37}TS​=[sR0T​t1​](3-37)

旋转矩阵部分增加的缩放因子sss，表示在对向量旋转之后，还可以在x,y,zx, y, zx,y,z三个坐标上对物体均匀缩放。例如，边长为1的立方体通过相似变换后，变成边长为10的立方体。

1. 仿射变换

仿射变换的矩阵形式为：

TA=[At0T1](3-38)\mathbf{T}_{A} = \begin{bmatrix} \mathbf{A} & \mathbf{t} \\ \mathbf{0}^{\text{T}} & 1 \end{bmatrix} \tag {3-38}TA​=[A0T​t1​](3-38)

仿射变换要求A\mathbf{A}A是一个可逆矩阵，不必是正交矩阵。仿射变换也叫正交投影。例如，立方体经过仿射变换后，变成平行六面体。

1. 射影变换

射影变换形式最为一般：

TP=[AtaT1](3-39)\mathbf{T}_{P} = \begin{bmatrix} \mathbf{A} & \mathbf{t} \\ \mathbf{a}^{\text{T}} & 1 \end{bmatrix} \tag {3-39}TP​=[AaT​t1​](3-39)

其左上角为可逆矩阵A\mathbf{A}A、右上为平移t\mathbf{t}t、左下缩放aT\mathbf{a}^{\text{T}}aT。由于采用齐次坐标，当v≠0v \neq 0v​=0时，可以对矩阵各元素除以vvv得到右下角为1的矩阵；否则，得到右下角为0的矩阵。因此，2D的射影变换共有8个自由度，3D共有15个自由度。从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，这个变换则为仿射变换。

在“不变性质”中，从上到下是包含关系。例如，欧氏变换除了保体积之外，也具有保平行、相交等性质。

实践：Eigen几何模块

// use_geometry.cpp#include <iostream>
#include <cmath>using namespace std;#include <Eigen/Core>
#include <Eigen/Geometry>/**********************************
* 本程序演示了 Eigen 几何模块的使用方法
**********************************/int main(int argc, char const *argv[])
{/* code */// `Eigen/Geometry`模块提供各种旋转和平移表示// 3D旋转矩阵直接使用`Matrix3d`或`Matrix3f`Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();// 旋转向量使用`AngleAxis`，其底层不｀Matrix｀，但通过重载运算符，可以进行矩阵运算可// 沿Z轴旋转45度Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));cout.precision(3);// 用`matrix()`转换成矩阵cout << "rotation matrix = \n" << rotation_vector.matrix() << endl;// 直接赋值rotation_matrix = rotation_vector.toRotationMatrix();// 用`AngleAxis`进行坐标变换Eigen::Vector3d v(1, 0, 0);Eigen::Vector3d v_rotated = rotation_vector * v;cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;// 用旋转矩阵v_rotated = rotation_matrix * v;cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;// 欧拉角：将旋转矩阵转换成欧拉角// ZYX顺序，`yaw-pitch-roll`Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0);cout << "yaw pitch roll = " << euler_angle.transpose() << endl;// 欧氏变换矩阵使用`Eigen::Isometry`// 实质是4＊4的矩阵Eigen::Isometry3d T = Eigen::Isometry3d::Identity();// 按rotation_vector旋转T.rotate(rotation_vector);// 平移向量设成(1,3,4)T.pretranslate(Eigen::Vector3d(1, 3, 4));// 用变换矩阵进行坐标变换// R * v + tEigen::Vector3d v_transformed = T * v;cout << "v tranformed = " << v_transformed.transpose() << endl;// 仿射和射影变换，`Eigen::Affine3d`和`Eigen::Projective3d`// 四元数// `AngleAxis`可直接赋值四元数，反之亦然Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);// coeffs的顺序是`(x,y,z,w)`，`w`为实部，前三者为虚部cout << "quaternion = \n" << q.coeffs() << endl;// 旋转矩阵赋值四元数q = Eigen::Quaterniond(rotation_matrix);cout << "quaternion = \n" << q.coeffs() << endl;// 使用四元数旋转一个向量，用重载的乘法// 数学表达式为｀qvq^{-1}｀v_rotated = q * v;cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;return 0;
}

# CMakeLists.txt# 声明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )# 声明一个 cmake 工程
project( Eigen )# 添加头文件
include_directories( "/usr/include/eigen3" )# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable( use_geometry use_geometry.cpp )# 设置编译模式
set( CMAKE_BUILD_TYPE "Debug" )

p.s.

scipy部分实现了Eigen

scipy.spatial.transform.Rotation(quat, normalize=True, copy=True)类：实现3D旋转

• 接口：

  • 四元数（quaternion）

  • 旋转矩阵（rotation matrix）

  • 旋转向量（rotation vector）

  • 欧拉角（Euler angle）

• 支持：

  • Application on vectors

  • Rotation Composition

  • Rotation Inversion

  • Rotation Indexing

from scipy.spatial.transform import Rotation as R
import numpy as np

# 沿Z轴旋转45度
# 旋转向量
r = R.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))print(r.as_matrix())

[[ 0.70710678 -0.70710678  0.        ][ 0.70710678  0.70710678  0.        ][ 0.          0.          1.        ]]

v = np.array([1, 0, 0])# 旋转矩阵
rotation_matrix = r.as_matrix()
rotation_vector = r.as_rotvec()v_rotated = np.matmul(rotation_matrix, v)
print("(1,0,0) after rotation = ", v_rotated.T)v_rotated = r.apply(v)
print("(1,0,0) after rotation = ", v_rotated.T)

(1,0,0) after rotation =  [0.70710678 0.70710678 0.        ]
(1,0,0) after rotation =  [0.70710678 0.70710678 0.        ]

# 欧拉角
euler_angles = r.as_euler(seq="zyx", degrees=True)print("yaw pitch roll = ", euler_angles)

yaw pitch roll =  [45.  0.  0.]

视觉SLAM十四讲：第3讲 三维空间刚体运动相关推荐

1. 视觉SLAM总结——视觉SLAM十四讲笔记整理

   视觉SLAM总结--视觉SLAM十四讲笔记整理 说明 基础知识点 1. 特征提取.特征匹配 (1)Harris (2)SIFT (3)SUFT (4)ORB (5)特征匹配 2. 2D-2D:对极约束 ...

2. 浅读《视觉SLAM十四讲：从理论到实践》--操作1--初识SLAM

   浅读<视觉SLAM十四讲:从理论到实践>--操作1--初识SLAM 下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/s ...

3. 视觉SLAM十四讲（3）：三维空间刚体运动

   本章需要掌握的知识点有:旋转矩阵,变换矩阵,四元数,欧拉角定义和数学表达:同时也要掌握Eigen库关于矩阵.几何模块的使用方法. 文章目录 3.1 旋转矩阵 3.1.1 点,向量和矩阵的关系 3.1. ...

4. 视觉SLAM十四讲（2）：初识SLAM

   这一讲主要介绍视觉SLAM的结构,并完成第一个SLAM程序:HelloSLAM. 目录 2.1 小萝卜的例子 单目相机 双目相机 深度相机 2.2 经典视觉SLAM框架 2.3 SLAM问题的数学表述 ...

5. 视觉SLAM十四讲（1）：预备知识

   最近在学习高翔博士的<视觉SLAM十四讲>(第二版),算是初学本书,配套资源还算蛮丰富的,有代码(第一版和第二版都有),B站上也有高翔博士对第一版录制的讲解视频,真的是很贴心. 来吧,让我 ...

6. 视觉slam十四讲 pdf_视觉SLAM十四讲|第12讲 回环检测

   1. 什么是回环检测 前面有说过累积误差的问题,前一时刻的误差会积累到后面,导致画不成圈圈,如图12-1所示,而画圈圈(全局一致性)很重要,所以需要有一个步骤来纠正当前的计算偏差. 回环检测通过判断相 ...

7. 视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践

    专栏汇总 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第 ...

8. 视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵

   专栏系列文章如下:  专栏汇总 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLA ...

9. 视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理

   第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客 ​​​​​​视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...

10. 视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机

    第四讲---第五讲学习笔记如下: 视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义.指数和对数映射_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模 ...

最新文章

1. 如何高效地逛Github？
2. hadoop知识整理（4）之zookeeper
3. html5实现圆圈里带一个三角形,HTML5 Canvas圆圈里面的三角形变换动画
4. html里调用css的语句
5. 趣说游戏AI开发：曼哈顿街角的A*算法 1
6. 二维数组求最小值_05-最大子矩形-最大值减去最小值小于或等于num的子数组数量...
7. 【C语言简单说】十七：数组
8. xftp实现本地与服务器的文件上传下载（windows）
9. python相关性分析特征过滤_Python相关性分析
10. varnish 4.0 官方文档翻译17-Hashing
11. JAVA内存释放机制
12. proteus仿真 引脚显示电平变化但不能显示波形
13. ios版qq聊天记录的导出
14. 利用opencv-python绘制多边形框或（半透明）区域填充（可用于分割任务mask可视化）
15. 聚合数据--汇率接口调用
16. 统计学假设检验中 p 值的含义具体是什么？
17. 5v功放芯片哪个音质好
18. nrm是什么？以及nrm的安装与命令
19. java获取网络图片（比如微信授权后的头像）上传至linux服务器
20. SSM整合开发学习笔记

热门文章

1. 汇编指令 BCC/BLO
2. JS(解构) 之数组和对象中提取数据总结
3. arttemplate入门
4. 浪潮之巅读书笔记（二）
5. Idea中诡异的错误——文件为灰色并显示一个橙色时钟图标
6. 前端基础面试题(HTML + CSS)
7. 三星note20u计算机功能,【三星Note20U上手简单评测】
8. PyTorch数据归一化处理：transforms.Normalize及计算图像数据集的均值和方差
9. 计算机等级考试一级在线模拟,全国计算机等级考试一级模拟试题1
10. php utf8生僻字,支持生僻字且自动识别utf-8编码的php汉字转拼音类_PHP