开篇介绍

在上次博客中，我们介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。其中重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。于是我们把他归结为一种典型的优化问题，求解最优的 R \mathbf{R} R， t \mathbf{t} t，使得误差最小化。

一、群和李群

在上次谈到的三维旋转矩阵和变换矩阵中，我们知道三维旋转矩阵构成了特殊正交群 S O ( 3 ) SO(3) SO(3)，而变换矩阵构成了特殊欧氏群 S E ( 3 ) SE(3) SE(3)： S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3)=\left \{\mathbf{R}\in\mathbb{R}^{3\times3}\mid\mathbf{R}\mathbf{R}^T=\mathbf{I},det(\mathbf{R})=1 \right \} SO(3)={R∈R3×3∣RRT=I,det(R)=1} S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\left \{ \mathbf{T}=\begin{bmatrix} \mathbf{R}&\mathbf{t} \\ \mathbf{0}^T&1 \end{bmatrix}\in\mathbb{R}^{4\times 4}\mid\mathbf{R}\in SO(3),\mathbf{t}\in\mathbb{R}^3\right \} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
但是，旋转矩阵和变换矩阵对加法来说都是不封闭的。也就是说，对任意两个旋转矩阵 R 1 , R 2 R_1,R_2 R1​,R2​，按照矩阵加法的定义，和不再是一个旋转矩阵： R 1 + R 2 ∉ S O ( 3 ) R_1+R_2\notin SO(3) R1​+R2​∈/​SO(3)然而，它们对乘法有着封闭性： R 1 R 2 ∈ S O ( 3 ) , T 1 T 2 ∈ S E ( 3 ) R_1R_2\in SO(3),T_1T_2\in SE(3) R1​R2​∈SO(3),T1​T2​∈SE(3)
群的概念：群是一种集合加上一种运算的代数结构。集合记作 A \mathbf{A} A，运算记作 ⋅ \cdot ⋅，那么群可以记作 G = ( A , ⋅ ) G=(A,\cdot) G=(A,⋅)。
李群是指具有连续(光滑)性质的群。像整数群 Z \mathbb{Z} Z那样的群没有连续性质，所以不是李群。而 S O ( n ) SO(n) SO(n)和 S E ( n ) SE(n) SE(n)，它们在实数空间上是连续的。我们能够直观的想象一个刚体能够在连续地空间中运动，所以它们是李群。

二、李代数

I.李代数的介绍

对于任意旋转矩阵，我们知道其也是一个正交矩阵，满足： R R T = I \mathbf{R}\mathbf{R}^T=\mathbf{I} RRT=I在实际应用场景下， R \mathbf{R} R是机器人或小车的旋转，会随着时间连续变化，于是旋转矩阵就为时间的连续函数 R ( t ) \mathbf{R}(t) R(t)。于是有： R ( t ) R ( t ) T = I \mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I} R(t)R(t)T=I，由于李群是连续的，对时间求导后得： R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 \dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=0 R˙(t)R(t)T+R(t)R˙(t)T=0 R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T)^T R˙(t)R(t)T=−(R˙(t)R(t)T)T
我们可以看出 R ˙ ( t ) R ( t ) T \dot{\mathbf{R}}(t)\mathbf{R}(t)^T R˙(t)R(t)T是一个反对称矩阵，根据先前介绍的叉乘运算，我们引入了向量转反对称矩阵的运算(也即 ∧ ^\wedge ∧)，对于任意反对称矩阵，我们都能找到一个与之对应的向量。于是我们令： R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ {\color{Cyan} \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^\wedge} R˙(t)R(t)T=ϕ(t)∧，根据正交阵的特性，其逆矩阵为自己的转置矩阵，于是等式两边右乘 R ( t ) \mathbf{R}(t) R(t)有： R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) \dot{\mathbf{R}}(t)=\phi(t)^\wedge\mathbf{R}(t)=\begin{bmatrix} 0&-\phi_3&\phi_2 \\ \phi_3&0&-\phi_1 \\ -\phi_2&\phi_1 &0 \end{bmatrix}\mathbf{R}(t) R˙(t)=ϕ(t)∧R(t)=⎣⎡​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​⎦⎤​R(t)可见，每对旋转矩阵求导一次，我们只需左乘一个 ϕ ( t ) ∧ \phi(t)^\wedge ϕ(t)∧矩阵即可。假设旋转矩阵满足 R ( 0 ) = I \mathbf{R}(0)=\mathbf{I} R(0)=I，我们对 R ( t ) \mathbf{R}(t) R(t)在 t = 0 t=0 t=0处进行一阶泰勒展开： R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) \mathbf{R}(t)\approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\phi(t_0)^\wedge(t) R(t)≈R(t0​)+R˙(t0​)(t−t0​)=I+ϕ(t0​)∧(t)
在 t 0 t_0 t0​附近，假设 ϕ \phi ϕ保持为常数 ϕ ( t 0 ) = ϕ 0 \phi(t_0)=\phi_0 ϕ(t0​)=ϕ0​，那么通过求解 R \mathbf{R} R的微分方程，解得 R ( t ) = e x p ( ϕ 0 ∧ t ) {\color{Magenta} \mathbf{R}(t)=exp(\phi_0^\wedge t)} R(t)=exp(ϕ0∧​t)
不失一般性，我们找出了给定某时刻的 R \mathbf{R} R，我们就能求得一个 ϕ \phi ϕ，它描述了 R R R在局部的导数关系，那么这里的 ϕ \phi ϕ就是我们 S O ( n ) SO(n) SO(n)群的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)。
在变换矩阵中，我们如此类推，也能得到 S E ( n ) SE(n) SE(n)群的李代数 s e ( 3 ) \mathfrak{se}(3) se(3)。

II.指数映射

在清楚了旋转矩阵或者变换矩阵能够构成对应的李群后，我们也清楚了它们各自的李代数，也就是旋转向量 ϕ = θ a \phi=\theta\mathbf{a} ϕ=θa，其中，这里 a \mathbf{a} a是长度为1的方向向量， θ \theta θ描述了旋转向量的长度。接下来我们就能通过以 e e e为底数的指数函数的泰勒展开式来进行推导，最后得出 R = e x p ( ϕ ∧ ) = e x p ( θ a ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ {\color{Magenta} \begin{aligned} \mathbf{R} &=exp(\phi^\wedge) \\ &= exp(\theta\mathbf{a}^\wedge) \\ &=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{aa}^T+\sin\theta\mathbf{a}^\wedge \end{aligned}} R​=exp(ϕ∧)=exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧​这也就是我们之前提到过的罗德里格斯公式，详细的推导证明可以查看——罗德里格斯公式推导
而当我们知道了旋转矩阵，需要求出旋转向量的时候，我们可以通过旋转矩阵的迹公式求出旋转角 θ \theta θ以及通过求 R \mathbf{R} R的特征向量求出旋转向量的方向。
同样地，变换矩阵的指数映射求解也能从相同的推导中得出，过程大同小异，只是引入了平移量。

三、BCH公式及近似

我们使用李代数的一个动机是为了进行优化，而在优化问题中导数又是重中之重的任务。所以我们来考虑在 S O ( 3 ) SO(3) SO(3)群中的两个矩阵的乘法，是否能对应到李代数 s o ( 3 ) \mathfrak{so}(3) so(3)中的加法呢？如果成立的话，相当于： e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) = e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) exp(\phi_1^\wedge)exp(\phi_2^\wedge)=exp((\phi_1+\phi_2)^\wedge) exp(ϕ1∧​)exp(ϕ2∧​)=exp((ϕ1​+ϕ2​)∧)
但是，很遗憾，该式只在变量是标量的情况下成立；在变量为矩阵时并不成立。
两个李代数指数映射乘积的完整形式，由 B a k e r − C a m p b e l l − H a u s d o r f f Baker-Campbell-Hausdorff Baker−Campbell−Hausdorff公式给出。在 B C H BCH BCH公式下，当扰动量很小时，我们可以分别对应到左乘模型和右乘模型： ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2  if  ϕ 1 i s s m a l l J l ( ϕ 1 ) − 1 ϕ 2 + ϕ 1  if  ϕ 2 i s s m a l l \ln(\exp(\phi_1^\wedge)\exp(\phi_2^\wedge))^\vee \approx \begin{cases} \mathbf{J}_l(\phi_2)^{-1}\phi_1+\phi_2 & \text{ if } \phi_1 is \quad small \\ \mathbf{J}_l(\phi_1)^{-1}\phi_2+\phi_1 & \text{ if } \phi_2 is \quad small \end{cases} ln(exp(ϕ1∧​)exp(ϕ2∧​))∨≈{Jl​(ϕ2​)−1ϕ1​+ϕ2​Jl​(ϕ1​)−1ϕ2​+ϕ1​​ if ϕ1​issmall if ϕ2​issmall​
上式中第一个对应的是左乘模型(或左扰动模型)，第二个对应的是右乘模型。(或右扰动模型)

四、李代数求导

当一切准备就绪之后，我们就要考虑我们的优化问题了。回顾之前，我们引入了具有连续性质的李群，并知道了李代数的求解方法。由于实际场景应用当中，每个观测点得到的位姿数据都会存在噪声，于是我们要得到机器人或者小车在什么位置能够得到最佳观测数据，也就是归结到我们的优化问题上了。
于是，我们的目标就是，优化旋转矩阵，得到最佳的观测位置。 z = T p + w \mathit{z=Tp+w} z=Tp+w e = z − T p \mathit{e=z-Tp} e=z−Tp
现在，我们通过对李代数求导， ∂ ( R p ) ∂ R = ∂ ( exp ⁡ ( ϕ ∧ ) p ) δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( ϕ + δ ϕ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ ≈ lim ⁡ δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( J l δ ϕ ) ∧ exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 − ( exp ⁡ ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l {\color{Magenta}\begin{aligned} \frac{\partial (Rp)}{\partial R} &= \frac{\partial(\exp(\phi^\wedge)p)}{\delta\phi}\\ &= \lim_{\delta\phi\rightarrow 0}\frac{\exp((\phi+\delta\phi)^\wedge)\mathit{p}-\exp(\phi^\wedge)\mathit{p}}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow0}\frac{\exp(\mathit(J_l\delta\phi)^\wedge)\exp(\phi^\wedge)\mathit{p}-\exp(\phi^\wedge)\mathit{p}}{\delta\phi}\\ &\approx\lim_{\delta\phi\rightarrow 0}\frac{(I+(J_l\delta\phi)^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow 0}\frac{(J_l\delta\phi)^\wedge\exp(\phi^\wedge)p}{\delta\phi}\\ &=\lim_{\delta\phi\rightarrow 0}\frac{-(\exp(\phi^\wedge)p)^\wedge J_l\delta\phi}{\delta\phi}\\ &= -(Rp)^\wedge J_l \end{aligned}} ∂R∂(Rp)​​=δϕ∂(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​​

五、扰动模型

以上述介绍过的左乘模型为例子，对 R R R的一次扰动量设为 Δ R \Delta R ΔR。这个扰动可以左乘或右乘，最后结果会由细微的差异。我在这里先以左扰动为例，设左扰动 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ，然后对 φ \varphi φ求导，即：
∂ ( R p ) ∂ φ = lim ⁡ φ → 0 exp ⁡ ( φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ ≈ lim ⁡ φ → 0 ( 1 + φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ R p φ = lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ {\color{blue}\begin{aligned} \frac{\partial(Rp)}{\partial\varphi} &=\lim_{\varphi\rightarrow 0}\frac{\exp(\varphi^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\varphi}\\ &\approx\lim_{\varphi\rightarrow 0}\frac{(1+\varphi^\wedge)\exp(\phi^\wedge)p-\exp(\phi^\wedge)p}{\varphi}\\ &=\lim_{\varphi\rightarrow 0}\frac{\varphi^\wedge Rp}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{-(Rp)^\wedge\varphi}{\varphi}=-(Rp)^\wedge \end{aligned}} ∂φ∂(Rp)​​=φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​≈φ→0lim​φ(1+φ∧)exp(ϕ∧)p−exp(ϕ∧)p​=φ→0lim​φφ∧Rp​=φ→0lim​φ−(Rp)∧φ​=−(Rp)∧​
可见，扰动模型相比于直接对李代数求导，省去了一个雅可比 J l J_l Jl​的计算，这使得扰动模型更加实用。

六、Sophus实践

在了解到李群和李代数、李代数求导以及扰动模型后，我们在Kdevelop上进行基本的实践操作：(详情注释已经在代码中体现)

编译执行后得到如下结果：

这里使用的是非模板类的Sophus，在下载后非模板类Sophus后，我们需要在CMakeLists文本中加入寻找库的命令：

#为使用sophus，需要使用find_package命令找到它
find_package(Sophus REQUIRED)
project(useSophus)

include_directories(${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES})

另外要注意的一点是，在github源上的Sophus存在一些小的bug(https://github.com/strasdat/Sophus.git)
其中so2.cpp文件中有一个类出现了问题：

unit_complex_.real() = 1.;
unit_complex_.imag() = 0.;

应该修改为：

unit_complex_.real(1.);
unit_complex_.imag(0.);

最终，Sophus才能编译成功，开发者才能成功使用Sophus库开发项目。

七、总结

到目前为止，SLAM中的系统的位置姿态表述基础知识已经完全叙述完了，之前提到的旋转向量、旋转矩阵、四元数、欧拉角还有李代数和李群等等基本构件，都是SLAM中必不可少的元件，通过初步接触和学习，希望在后续开发中得到深入与场景应用。