三维空间刚体运动
SLAM数学表达
xk=f(xk−1,uk,wk)
这里uk是运动传感器的读数（有时也叫输入），wk为噪声。我们把它称为
运动方程。
与运动方程相对应，还有一个观测方程。当小萝卜在xk位置上
看到某个路标点yj，产生了一个观测数据zk,j。
zk,j=h(yj,xk,vk,j)
这里vk,j是这次观测里的噪声。
旋转矩阵
点和向量，坐标系
三维空间中的某个向量的坐标可以用R3当中的三个数来描述。如果我们确定一个坐标系，那就可以谈论向量a 在这组基下的坐标了：
a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
内积可以描述向量间的投影关系。而外积呢是这个样子:
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
我们引入了人符^号，把a写成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric),你可以将^记成一个反对称符号。这样就把外积a×b, 写成了矩阵与向量的乘法a∧b,把它变成了线性运算。
坐标系间的欧氏变换
相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会 发生变化。这种变换称为欧氏变换。
首先来考虑旋转。我们设某个单位
正交基(e1,e2,e3)经过一次旋转，变成了(e1′,e2′,e3′)。那么，对于同一个向量a （注意该 向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为[a1,a2,a3]T和
[a1′,a2′,a3′]T。根据坐标的定义，有：
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
所以：
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
可以说，矩阵R描述了旋转本身。因此它又称为旋转矩阵。
变换矩阵与齐次坐标
[a′1]=[R0Tt1][a1]≜T[a1]
我们把一个三维向量的末尾添加1，变成了四维向量，称为齐次
坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成
了线性关系。该式中，矩阵©称为变换矩阵(Transform Matrix)。我们暂时用N 表示a 的齐次坐标。
旋转向量和欧拉角
任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使
用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量(或轴角，Axis-Angle)。
而欧拉角则提供了一种非常直观的 方式来描述旋转—— 它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。
- 绕物体的Z 轴旋转，得到偏航角yaw；
- 绕旋转之后的Y 轴旋转，得到俯仰角pitch；
- 绕旋转之后的X 轴旋转，得到滚转角roll。
四元数
旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的, 但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。一个四元数q 拥有一个实部和三个虚部。
q=q0+q1i+q2j+q3k
其中i,j,k为四元数的三个虚部。这三个虚部满足关系式:
⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
我们知道单位四元数能够表达三维空间的旋转。这种表达方式和旋转矩
阵、旋转向量有什么关系呢？
q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
李群与李代数
李群与李代数的定义
上一讲，我们介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧 拉角、四元数等若干种方式。我们重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题。
群 (Group)是一种集合加上一种运算的代数结构。我们把集合记作4 , 运算记作・, 那么群可以记作G=(A,⋅), 群要求这个运算满足以下几个条件：
-
封闭性: ∀a1,a2∈A,a1⋅a2∈A
-
结合律: ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
-
幺元: ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
-
逆: ∀a∈A,∃a−1∈A, s.t. a⋅a−1=a0
每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定义如下：
李代数由一个集合V , —个数域F 和一个二元运算[,]组成。如果它们满足以下几条
性质，称(V,F,[,])为一个李代数，记作g。
- 封闭性 ∀X,Y∈V,[X,Y]∈V
- 双线性 ∀X,Y,Z∈V,a,b∈F, 有 :
$
[a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}]
$
- 自反性 ∀X∈V,[X,X]=0
- 雅可比等价 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0
其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。
作为例子，三维向量上定义的叉积是一种李括号，g=(R3,R,×)构成了一个李代数。
指数与对数映射
SO(3) 上 的 指 数 映 射
根据前面的推导，每个 ϕ 都可以生成一个反对称矩阵：
Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3
在此定义下，两个向量 ϕ1,ϕ2 的李括号为:
[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
李群为：
so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
现在，我们说，R 是某个相机的旋转，它会随时间连续地变化，即为时间的函数：R(t)由于它仍是旋转矩阵，有
R(t)R(t)T=I
在等式两边对时间求导：
R˙(t)R(t)T=−(R˙(t)R(t)T)T
引入了^符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，我们亦能找 到一个与之对应的向量。把这个运算用符号∨表示：
a∧=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,A∨=a
于是，由于 R˙(t)R(t)T 是一个反对称矩阵，我们可以找到一个三维向量 ϕ(t)∈R3 与
之对应。于是有：
R˙(t)R(t)T=ϕ(t)∧
等式两边右乘 R(t), 由于 R 为正交阵，有
R˙(t)=ϕ(t)∧R(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤R(t)
上式是一个关于 R 的微分方程，而且我们知道初始值 R(0)=I, 解之，得:
R(t)=exp(ϕ0∧t)
exp(ϕ∧) 是如何计算的？它是一个矩阵的指数，在李群和李 代数中，称为指数映射(ExponentialMap)。
任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果, 其结果仍是一个矩阵。
exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
我们来仔细推导一下这个定义。由于 ϕ 是三维向量，我们可以定义它的模长和它的
方向，分别记作 θ 和 a, 于是有 ϕ=θa∘ 这里 a 是一个长度为 1 的方向向量。首先，对
于 a∧, 有以下两条性质:
a∧a∧=aaT−Ia∧a∧a∧=−a∧
它们提供了处理a∧高阶项的方法。利用这两个性质, 我们可以把指数映射写成：
exp(ϕ∧)=exp(θa∧)=n=0∑∞n!1(θa∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+…=aaT−a∧a∧+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4(a∧)2+…=aaT+(θ−3!1θ3+5!1θ5−…)a∧−(1−2!1θ2+4!1θ4−…)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧
最后我们得到罗德里格斯公式:
exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧
SE (3)上的指数映射
对于 SE(3), 它也有对应的李代数 se(3)∘ 为省略篇幅，我们就不描述如何引出 se(3)
了。与 so(3) 相似，se(3) 位于 R6 空间中:
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
我们把每个 se(3) 元素记作 ξ, 它是一个六维向量。前三维为平移，记作 ρ; 后三维为旋转，记作 ϕ, 实质上是 so(3) 元素。同时，我们拓展了∧符号的含义。在 se(3) 中, 同样使用 ^ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称:
ξ∧=[ϕ∧0Tρ0]∈R4×4
下面我们来介绍 se(3) 上的指数映射。为了节省篇幅，我们不再像 so(3) 那样详细推 导指数映射。se(3) 上的指数映射形式如下:
exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]≜[R0TJρ1]=T
如果你有耐心，可以照着 so(3) 上的做法推导，把 exp 进行泰勒展开推导此式。从结 果上看，ξ的指数映射左上角的 R 是我们熟知的 so(3) 中的元素，与 se(3) 当中的旋转部
分 ϕ 对应。而右上角的 J 则可整理为（设 ϕ=θa):
J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
李代数求导与扰动模型
BCH公式与近似形式
BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由
李括号组成的余项。
In the particular case of SO(3), we can show that
ln(C1C2)∨=ln(exp(ϕ1∧)exp(ϕ2∧))∨=ϕ1+ϕ2+21ϕ1∧ϕ2+121ϕ1∧ϕ1∧ϕ2+121ϕ2∧ϕ2∧ϕ1+⋯
where C1=exp(ϕ1∧),C2=exp(ϕ2∧)∈SO(3). Alternatively, if we as-
sume that ϕ1 or ϕ2 is small, then using the approximate BCH formulas we can show that
ln(C1C2)∨=ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jℓ(ϕ2)−1ϕ1+ϕ2ϕ1+Jr(ϕ1)−1ϕ2 if ϕ1 small if ϕ2 small
本书以左乘为例。左乘 BCH 近似雅可比 Jl实际上就是SE(3)上的指数映射中的J
Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
它的逆为:
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
而右乘雅可比仅需要对自变量取负号即可：
Jr(ϕ)=Jl(−ϕ)
假定对某个旋转 R，对应的李代数为 ϕ 。我们给它左乘一个微小旋转，记作 ΔR 对应的李代数为 Δϕ。 那么，在李群上，得到的结果就是 ΔR⋅R, 而在李代数上，根据 BCH 近似，为: Jl−1(ϕ)Δϕ+ϕ 。合并起来，可以简单地写成:
exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
反之，如果我们在李代数上进行加法，让一个 ϕ 加上 Δϕ, 那么可以近似为李群上带 左右雅可比的乘法:
exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
这将为之后李代数上的做微积分提供了理论基础。同样的，对于 SE(3)，亦有类似的 BCH 近似公式:
exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)
exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧)
SO(3)李代数上的求导
下面我们来讨论一个带有李代数的函数，如何关于该李代数求导的问题。该问题有很
强的实际背景。在 SLAM中，我们要估计一个相机的位置和姿态，该位姿是由SO(3)的旋转矩阵或SE(3)上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为它观察到了一个世界坐标位于p的点，产生了一个观测数据z 。那么，由坐标变换关系知：
z=Tp+w
其中w为噪声。我们通常会计算理想的观测与实际数据的误差：
e=z−Tp
假设一共有 N 个这样的路标点和观测，于是就有 N 个上式。那么，对小萝卜的位姿估计，相当于是寻找一个最优的 T，使得整体误差最小化：
TminJ(T)=i=1∑N∥zi−Tpi∥22
我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，SO(3),SE(3)上并没有良好定义的加法，它们只是群。如果我们把T当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：
- 用李代数表示姿态，然后对根据李代数加法来对李代数求导。
- 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。
首先，考虑SO(3)上的情况。假设我们对一个空间点p进行了旋转，得到了Rp。现在，要计算旋转之后点的坐标相对于旋转的导数，由于 SO(3) 没有加法，所以该导数无法按?导数的定义进行计算。设 R 对应的李代数为 ϕ, 我们转而计算:
∂ϕ∂(exp(ϕ∧)p)
按照导数的定义，有：
∂ϕ∂(exp(ϕ∧)p)=δϕ→0limδϕexp((ϕ+δϕ)∧)p−exp(ϕ∧)p=δϕ→0limδϕexp((Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(I+(Jlδϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p=δϕ→0limδϕ(Jlδϕ)∧exp(ϕ∧)p=δϕ→0limδϕ−(exp(ϕ∧)p)∧Jlδϕ=−(Rp)∧Jl
第二行的近似为BCH线性近似，第三行为泰勒展开舍去高阶项后近似，第四行至第五行将反对称符号看作叉积，交换之后变号。于是，我们推导了旋转后的点相对于李代数的导数：
∂ϕ∂(Rp)=(−Rp)∧Jl
扰动模型（左乘）
另一种求导方式，是对$ R $进行一次扰动 ΔR。 这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 ΔR 对应的李代数 为φ。然后，对 φ 求导，即:
∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p≈φ→0limφ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
可见，扰动模型相比于直接对李代数求导，省去了一个雅可比Jl的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计当中具有重要的意义。
SE(3)上的李代数求导
最后，我们给出 SE(3) 上的扰动模型，而直接李代数上的求导就不再介绍了。假设 恭空间点 p 经过一次变换 T (对应李代数为 ξ), 得到 Tp 。现在，给 T 左乘一个扰动 ΔT=exp(δξ∧), 我们设扰动项的李代数为 δξ=[δρ,δϕ]T, 那么：
∂δξ∂(Tp)=δξ→0limδξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p≈δξ→0limδξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p=δξ→0limδξδξ∧exp(ξ∧)p=δξ→0limδξ[δϕ∧0Tδρ0][Rp+t1]
=δξ→0limδξ[δϕ∧(Rp+t)+δρ0]=[I0T−(Rp+t)∧0T]≜(Tp)⊙