1概述

本章主要涉及到三维刚体运动，包括向量的运算、旋转矩阵和旋转向量、变换矩阵、四元数、李群和李代数以及李代数的求导。主要的难点在李群和李代数部分，而向量的乘法和变换矩阵的一些基础知识后续会持续用到。李代数的主要目的是为了解决李群对加法不封闭的问题（无法使用常规的方法求极限，故而无法求导）。将李群转换为李代数，而李代数对加法是封闭的，能够解决李群的求导问题。

2向量点积和叉积

2.1向量的点积

向量的点积也叫内积，向量的点积结果是一个标量，是其中一个向量投影到另外一个向量上，然后做数乘。
$a \cdot b = {a^T}b = \sum\limits_{i = 1}^3 {{a_i}{b_i}} = \left| a \right| \cdot \left| b \right|\cos \left\langle a \right.,\left. b \right\rangle$a⋅b=aTb=i=1∑3​ai​bi​=∣a∣⋅∣b∣cos⟨a,b⟩

2.2 向量的叉积

向量的叉积也叫向量的外积，计算结果还是一个向量，这个新向量垂直于原始的两个向量。

其中这里定义了反对称矩阵，用于把向量变换为一个特殊的矩阵。

也定义了反对称矩阵A到向量的变换符号：

这里还有一些值得注意的演变公式，在后期很重要：
$a \times b = {a^ \wedge }b = - {b^ \wedge }a$a×b=a∧b=−b∧a
由于a和b都和叉积垂直，所以：
${a^T}{a^ \wedge }b = {b^T}{a^ \wedge }b = 0$aTa∧b=bTa∧b=0

3 旋转和变换

3.1 变换矩阵与齐次坐标

旋转矩阵也叫作方向余弦矩阵，这里不再推导。给出一些约束条件，旋转矩阵的充分必要条件是该矩阵是一个行列式为1的正交矩阵，对于n维旋转矩阵定义如下：
$SO(n) = \left\{ {R \in {\Re ^{n \times n}}|R{R^T} = I,\det (R) = 1} \right\}$SO(n)={R∈ℜn×n∣RRT=I,det(R)=1}
SO(n)是特殊正交群(Special Orthogonal Group)的意思。显然，一个旋转的相反变换为：

平移比旋转简单很多，在原坐标上加上平移向量即可实现平移。

这里要注意的是，按上式，变换是先选择再平移，显然，a’也做先旋转R.T在平移t是得不到a的。
上面的变换公式不是一个线性关系，多次变换时，由于t的存在，会很繁琐，比如：
$b = {R_1}a + {t_1},c = {R_2}b + {t_2}$b=R1​a+t1​,c=R2​b+t2​
那么a到c的变换为：
$c = {R_2}({R_1}a + {t_1}) + {t_2}$c=R2​(R1​a+t1​)+t2​
多次变换很麻烦。所以引入齐次坐标：

向坐标的末尾添加一个维度，这个维度恒为常数1，将三维坐标变成四维，称为齐次坐标。而矩阵T称为变换矩阵，它使得坐标变换变成了线性关系，等式变得简洁。这个矩阵称为特殊欧氏群(Special Euclidean Group)：

3.2 旋转向量

刚体某一次发生旋转，比如是绕着某个轴旋转了某个角度，因此可以定义一个单位向量，用来表示旋转的轴，而在单位向量上乘以常数θ表示旋转的角度，这样的一个单位长度为θ，方向为n的单位向量称为旋转向量。
$\Phi = \theta n$Φ=θn
旋转向量只用了3个量就表示了旋转，比旋转矩阵精简了很多。从旋转向量到旋转矩阵的变换过程由罗德里格斯公式（Rodrigues’s Formula）给出：
$R = \cos \theta I + (1 - \cos \theta )n{n^T} + \sin \theta {n^ \wedge }$R=cosθI+(1−cosθ)nnT+sinθn∧
旋转矩阵到旋转向量的旋转角度：

关于转轴n，显然旋转轴上的向量旋转后不会发生改变，这说明：
$Rn = n$Rn=n
因此，转轴n是矩阵R特征值为1的特征向量。求R的特征向量，并找出特征值为1的那一个特征向量，归一化，即可得到转轴。

3.3 四元数

4 李群和李代数

4.1 群

在前面提到SO(3)是特征正交群，SE(3)是特殊欧氏变换群，他们分别是旋转矩阵和变换矩阵。他们对乘法是封闭的，但是对加法并不封闭：
${R_{^1}} + {R_{^2}} \notin SO(3),{T_{^1}} + {T_{^2}} \notin SE(4)$R1​+R2​∈/​SO(3),T1​+T2​∈/​SE(4)
而求极限或者求导的时候，需要使用到自变量的加法（比如求解误差关于变换矩阵T的导数），因为这两个群对加法不封闭，因此无法求解他们的导数。
李群指的是光滑连续的群，空间中旋转和变换都是联系的，因此他们也是李群。
群是一种集合加上一种运算的代数结构。我们把集合记作A，把运算记作·，那么群可以记作G=(A,·)。群有几个限制条件：

4.2 李代数

李代数是李群在单位元处的正切空间，它反映了李群的导数性质。事实上，SO(3)的李代数so(3)正好是旋转矩阵R所对应的的旋转向量。so(3)是一个三维的向量，se(3)是一个六维的向量，它们对加法封闭，可以很方便地求导。
下面直接给出李群和李代数的变换关系：

可以看到李群到李代数的变换，就是求旋转矩阵R的旋转向量的过程，而李代数到李群的变换就是罗德里格斯公式。这再次说明so(3)就是SO(3)的旋转向量。

SE（3）则要复杂一些，它包含了SO(3)部分的变换。李群到李代数的变换，增加了平移部分，求解平移部分的李代数ρ，这个等式中，t和J都是可以得到的，解方程组就可以得到ρ。李代数到李群的变换中，比SO(3)也是多了平移的Jρ，求解即可。
值得注意的是，这里的变换T是针对齐次坐标系的。

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式

前面提到，李群对加法不封闭，因此无法直接求导。而李群在使用的时候，我们只使用乘法，那么两个李群相乘时，对应的李代数发生了什么呢。这个关系由BCH公式给出：

其中[]为李括号。BCH公式说明，当两个矩阵的指数（旋转矩阵）做乘法时，他们会产生一些由李括号组成的余项。当A或者B为小量时，二次以上的项可以忽略，给出近似公式：

其中φ是李代数。
该式子告诉我们，当对一个旋转R左乘一个小量时，可以近似地看作，在原有李代数上加上一个项，类似于泰勒展开一样。上面给出了左乘模型和右乘模型，这里只考虑左乘模型。事实上，这里的Jl就是前面李群和李代数互相变换中的J。上面式子整理下也可以写成：

对于se(3)上面的式子同样成立，只是需要把φ换成ξ，且J的形式更加复杂，这里它是一个6×6的矩阵。

4.3.2 李代数求导

我们经常需要求误差关于位姿或者运动T的导数，假设有世界坐标系下的点p，相机观测到的坐标是z，现在求相机的位姿，那么变换关系为：

其中w是随机噪声。由于他的存在，我们一般会让下面的误差尽量小来求T：
$e = z - Tp$e=z−Tp
假设有N个这样的点，那么就是求最优的T，使得整体误差最小：
$\mathop {\min }\limits_T J(T) = \sum\limits_{i = 1}^N {\left\| {{z_i} - T{p_i}} \right\|} _2^2$Tmin​J(T)=i=1∑N​∥zi​−Tpi​∥22​
这是典型的最小二乘问题，那么我们就要求误差关于T的雅克比矩阵。求导的思路有两种：
① 用李代数表示姿态，然后根据李代数加法对李代数求导
② 对李群左乘或右乘微小扰动，然后对扰动求导。
下面先用李代数求导来做。
为了简单，考虑只有旋转的情况，也就是SO(3)的情况。假设点p进行了旋转，得到了RP，要计算旋转后的点坐标关于旋转的导数，记为：
${{\partial (Rp)} \over {\partial R}}$∂R∂(Rp)​
由于SO(3)没有加法，所以该导数无法按照导数的定义进行求导。设R的李代数为φ，故有：

第二行是BCH近似，第三行是泰勒展开近似，最后一行利用了叉积的性质。
这里的Jl就是前面提到的J。形式比较复杂。

4.3.3 扰动模型

对R左乘一个小的扰动△R，看结果相对于扰动的变化率。下面设△R的李代数为φ，然后对φ求导：

可见，扰动模型得到的导数比直接求导的要少了Jl，更加简洁。
对于SE(3)直接给出扰动模型的计算，设空间点p经过一次变换T（李代数为ξ），得到Tp，现在给T左乘一个扰动△T=exp(δξ^)，我们设扰动的李代数为：
$\delta \xi = {\left[ {\delta \rho ,\delta \phi } \right]^T}$δξ=[δρ,δϕ]T