视觉slam十四讲学习笔记(2)

李群李代数

由于在slam中位姿是未知的，所以需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的R，t，使得误差最小化。
然而，旋转就很自身是存在约束的（正交且行列式为1）。当它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群李代数的转换，希望将其变为无约束的优化问题，简化求解。

李群李代数基础

三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧氏群SE(3):
旋转矩阵以及变换矩阵对于加法是不封闭的，也就是说任意两个旋转矩阵相加，按照矩阵加法的定义相加，和不再是一个旋转矩阵：

所以针对这两种矩阵加法并没有良好的定义，然而，乘法相对就是一种较好的运算，SO(3)和SE（3）关于乘法是封闭的。

两个旋转矩阵相乘表示做了两次旋转，对于这种只有一个运算的集合， 称为群。

群

群是一种集合加上一种运算的代数结构，将集合记做A，运算记做*，那么群可以记做G = （A，*）。群要求这个运算满足一下条件：

矩阵中常见的群一般有：

群结构保证了在群上的运算具有良好的性质。
李群指具有连续（光滑）性质的群。像整数群Z那样离散的群没有连续性质，不是李群。然而SO(n)和SE(n)，在实数空间上是连续的。可以直观的想象一个刚体能够连续地在空间中运动，所以它们都是李群。

李代数的引出

对于任意旋转矩阵R，满足：

现在，假设R是某个相机的旋转，它会随时间连续地变换，即为时间的函数：R(t)。由于它仍是旋转矩阵所以有：

在等式两边对时间求导：

整理得：

可以看出它是一个反对称矩阵。
∧符号表示将一个向量做叉积，变为反对称矩阵。对于任意反对称矩阵，也能找到一个与之对应的向量，这个运算符是∨。

由反对称矩阵可得：

等式两边右乘R(t),由于R为正交阵，有：

所以每对旋转矩阵求一次导数，只需左乘一个Φ^（t）矩阵即可。
假设t0 = 0，并设此时旋转矩阵为R(0) = I。按照导数定义，可以把R(t)在0附近进行一阶泰勒展开：

Φ反映了R的导数性质，所以称它为SO(3)远点附近的正切空间(Tangent Space)。 同时在t0附近，设Φ保持为常数Φ(t0) = Φ0。 那么可以得到：

由于初始值R(0) = I，可以解得：

由于做了一定假设，所以只在t=0附近有效。
可以看到，旋转矩阵R与另一个反对称矩阵Φ0通过指数关系发生了练习，也就是说，对于某个时刻的R，存在一个向量Φ，满足这个指数关系。

李代数的定义

每个李群都有与之对应的李代数，李代数描述了利群的局部性质。
李代数由一个集合V，一个数域F和一个二元运算[,]组成。如果它们满足以下几个条件，称（V，F，[,]）为一个李代数，记做g。

其中二元运算被称为李括号。
李括号表达了两个元素的差异，它不要求结合律，而要求元素和自己做李括号之后为零的性质。

李代数so(3)

Φ事实上是一种李代数。SO(3)对应的李代数是定义在三维空间上的向量，记做Φ。每个Φ都可以生成一个反对称矩阵：

在此定义下，两个向量的李括号为：

一般说so(3)的元素是3维向量或者3维反对称矩阵，不加区别。

它与SO(3)的关系由指数映射给定：

李代数se(3)

对于SE(3),也有对应的李代数，se(3)位于六维实数空间中：

把每个se(3)元素记做ξ，它是一个六维向量。前三维为平移，记做ρ，后三维为旋转记做Φ，实质上是so(3)元素。同时，扩展了∧符号的含义。在se(3)中，同样使用∧符号，将一个六维向量转换为四维矩阵，但这里不在表示反对称。

李代数se(3)也有类似于so(3)的李括号：

指数与对数映射

SO(3)上的指数映射

exp(Φ^)是一个矩阵的指数，在李群和李代数中，称为指数映射。
任意矩阵的指数映射可以写成一个泰勒展开，但是只有在瘦脸的情况下才会有结果，其结果仍是一个矩阵。

同样的，对so(3)中的任意一元素Φ，我们也可以按此方式定义它的指数映射：

由于Φ是三维向量，所以可以定义它的模长和方向，分别记做θ和α，于是有Φ = θα。这里α是一个长度为1的方向向量。首先对于α^,有以下两条性质。

这两个公式处理了α^高阶项的方法。利用这两个性质，我们可以把指数映射写成：

最终得到

这和罗德里格斯公式如出一辙，所以标识了so(3)实际上就是所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它，我们将so(3)中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。同样，利用对数映射，也可以完成反变换。

不过实际运算时，通常会通过旋转矩阵求解转角和转轴，可以很好的减少计算量。
指数映射只是一个满射。这意味着SO(3)中的元素，都可以找到一个so(3)z中的元素与之对应，但是可能存在多个so(3)中的元素与之对应；因为旋转具有周期性。但是，如果我们把旋转角度固定在正负180之间，那么李群和李代数元素就是一一对应的。
旋转就很的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

SE(3)上的指数映射

se(3)指数映射如下：

R是SO(3)中的元素，与se(3)当中的旋转部分Φ对应。J可以整理为：

李群李代数的转换关系

李代数求导与扰动模型

BCH公式与近似形式

研究SO(3)中的元素发生运算时so(3)发生了怎样的运算
两个李代数指数映射乘积的完整形式，又BCH公式给出。

其中[]为李括号。BCH公式可以看出，当两个矩阵指数乘积时，它们会产生一些由李括号组成的余项。
但是设定SO(3)上的李代数，当Φ1或Φ2为小量时，小量二次以上的项都可以被忽略掉。此时，BCH拥有线型近似表达：

李代数在BCH近似下，分成了左乘近似和右乘近似两种，使用时需注意，使用的是左乘模型还是右乘模型。
以左乘为例：
左乘BCH近似雅克比矩阵J1事实上就是前文的式子：

它的逆为：

右乘雅克比矩阵只需对自变量取负号即可：

左乘BCH也可以简单的写成：

在李代数上进行加法，也可以金思维李群上带左右雅克比的乘法：

这为李代数上的微分提供了基础。
对于SE(3)，也有类似的BCH近似公式：

SO(3)李代数上的求导

在SLAM中，我们要估计一个相机的位置和姿态，该位姿是由SO(3)的旋转就很或SE(3)上的变换矩阵描述的。设某个时刻相机的位姿为T，它观察到了一个世界坐标位于p的点，产生了一个观测数据z，那么，由坐标变换关系如：

然而，由于观测噪声w的存在，z往往不可能精确地满足z = TP的关系。所以，我们通常会计算理想的观测与实际数据的误差：

假设一共有N个这样的路标点和观测，于是就有N个上式。那么，对于相机的位姿估计，相当于是找一个最优的T，使得整体误差最小化：

所以需要计算目标函数J关于变换矩阵T的导数。
我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。
SO(3)，SE(3)上并没有良好定义的加法，所以优化时必须加以约束。而从李代数的角度来说，由于李代数由向量组成，具有良好的加法运算。所以，使用李代数解决问题一般分为两种：
1.用李代数表示姿态，然后对根据李代数加法来对李代数求导。
2.对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。
第一种方式对应求导模型
第二种方式对应扰动模型

李代数求导

首先，对于SO(3)来说,假设对一个空间点p进行了旋转，得到了Rp，计算旋转之后坐标对于旋转的导数，不严谨的记做。

由于没有加法定义，所以转为李代数：

推导得：

推导出了旋转后的点相对于李代数的导数：

但是Jl仍然很复杂，所以引出了更为简单的扰动模型。

扰动模型(左乘)

以左扰动为例，对左微小扰动对应的李代数求导

形式比上面的更为简单

SE(3)上的李代数求导

假设空间点p经过依次变换T(李代数为ξ)，得到Tp。现在左乘微小扰动。

最后的结果将一个齐次坐标的空间点变换成一个4×6的矩阵。