SLAMBOOK_3_4

三维空间刚体运动

目标：

理解三维空间的刚体运动描述方式：旋转、变换、四元数和欧拉角
掌握Eigen库的矩阵和几何模块

基本概念：
坐标：一方面与向量本身有关，另一方面与坐标系选取有关。
向量：可以是空间中的一样东西
外积可表示旋转：右手系叉乘
欧式变换：保证同一个向量在各个坐标系下的长度和夹角都不会变化，则可由一个旋转和平移组成
旋转矩阵的性质：行列式为1的正交矩阵，SO(n)是特殊正交群
齐次坐标：把旋转和平移写入一个矩阵，且保持整个关系为线性关系
变换矩阵T：左上为旋转，右侧为平移，左下为0，右下为1的特殊欧式群

旋转的表达

1.旋转矩阵：正交阵且行列式为1,行列向量相互正交；内积为0；

2.三个旋转轴和旋转角：使用一个向量，方向与旋转轴一致，长度等于旋转角，即李代数。由Rodrigues’s Formula可得：$R = cos\theta I + (1-cos\theta)nn^T + sin\theta n^`$ 其中n’是向量到反对称的转换符，从旋转矩阵到旋转向量转换$\theta = arccos(\frac{tr(R)-1}{2})$
转轴n是矩阵R特征值1对应的特征向量。解此方程归一化可得旋转轴。

3.欧拉角：分离成三次绕坐标轴旋转即yaw-pitch-roll.注意著名的万向锁问题，+-90度时，丢失一个自由度，产生奇异性问题。因此不适于插值和迭代，往往只用于人机交互。

4.四元数：既是紧凑的，也没有奇异性的旋转表达。缺点不够直观和运算复杂。
熟悉相互表达之间的转换。

相似、仿射、射影变换

1.相似： $T_s = [sR t]$, 7个自由度，xyz坐标上均匀缩放
2.仿射： $T_A = [A t]$, 12个自由度，A是可逆矩阵而不必是正交矩阵
3.射影： $T_p $, 15个自由度，最不规则的

// Eigen rotate data structure
Eigen::Matrix3d;  // 3*3 R matrix
Eigen::AngleAxisd;  // 3*1 R vector
Eigen::Vector3d;   // 3*1 Euler Angle
Eigen::Quaterniond;  // 4*1 quaternion
Eigen::Isometry3d;  // 4*4 T transformed
Eigen::Affine3d;   // 4*4 affine matrix
Eigen::Projective3d;  // 4*4 projective matrix

李群与李代数

目标：

理解概念，掌握SO(3),SE(3)
理解BCH近似意义
李代数的扰动模型
运用Sohpus对李代数运算

什么是Sophus? Sophus是Eigen的拓展，李代数库

为何引入李代数？旋转矩阵自身带有正交和行列式为1的约束，作为优化变量引入额外的约束使得优化困难。通过李群和李代数的转换关系，使位姿估计变成无约束的优化问题，简化求解方式。

数学基础

什么是群？一种集合加上一种运算的代数结构。封结幺逆
可以验证：

旋转矩阵集合和矩阵乘法构成群
变换矩阵和矩阵乘法也构成群
因此它们为旋转矩阵群和交换矩阵群

矩阵中常见的群：

一般线性群GL(n) : n*n的可逆矩阵，对矩阵乘法成群
特殊正交群SO(n) : 旋转矩阵群SO(2)和SO(3)
特殊欧式群SE(n) : 前面的n维欧式变换， SE(2)和SE(3)

什么是李群Lie Group？指具有连续（光滑）性质的群，既是群也是流形。整数群是离散的，不属于。而SO和SE在实数空间连续运动，所以属于李群。SO(3)和SE(3)只有定义良好的乘法，没有加法所以难以进行取极限和求导的操作。将旋转矩阵用李代数逼近迭代优化。

一种李代数（小写）对应一种李群（大写）。

$RR^T = I$ 对连续时间求导得 $ R^, R^T = -(R^, R^T )^T $, $R^,$为对时间t的导数。即满足反对称矩阵。最后得$R(t) = exp(\phi_0t)$
这表示：

1.给定某时刻的R，我们能求一个$\phi$描述R在局部的导数关系，正切空间邻域。$\phi$正是SO(3)上的李代数so(3)
2.矩阵指数如何计算？事实上这正是李群与李代数之间的指数/对数映射

什么是李代数？每个李群对应一个李代数，描述了李群单位元数的正切空间性质。满足封闭性、双线性、自反性、雅可比等价

什么是李括号？二元运算，表达了两个元素的差异。

指数与对数映射

// To do 后续补充，本章涉及较多数学和公式