slam中一个关键问题之一就是求解相机的位姿,人们找了很多以相机位姿为变量的误差函数,比如光度误差,重投影误差,3D几何误差等等,希望使得误差最小,进而求得比较准确的相机位姿。举一个重投影例子:
群是一种集合加上一种运算的代数结构,我们把集合记为A,运算记为 . ,那么群可以记为 G=(A,.) 。群要求这个运算要满足封闭性,结合律,幺元和可逆。矩阵中常见的群有:
群结构保证了在群上的运算具有良好的性子,李群是指具有连续性质的群。SO(n),SE(n)在实数空间上是连续的,我们可以直观的感觉到一个刚体能够在空间中连续运动,所以SO(n)和SE(n)都是李群。那么相机的位姿就可以表示为:
现在我们只是说明了相机位姿是特殊欧式群,旋转矩阵是特殊正交群。但是现在还不能直接去求解因为约束依然存在。正如前面所说的,我们希望将有约束的问题转化为无约束问题求解。数学上常用的方法就是去另外一个空间计算,就像分类问题一样,也许在这个空间没法分开,变换一个空间说不定就可以分开了。横看成岭侧成峰就是这个道理。数学家们研究表明每一个李群都已与之对应的李代数,因为李群是连续的。真如我们所期望的,在李代数求解1式时可以将其化为无约束问题,通过高斯牛顿法等方法迭代求解。这是因为旋转矩阵对加法不封闭,而李代数对加法封闭。
旋转矩阵对应的李代数是一个三维向量,相机位姿(刚性变换矩阵,没有尺度因子)是一个六位向量。那么我们想知道的是这些向量是怎么和对应的李群联系,或者说怎么变换。具体的推到我这就不在搬运了,大家可以参考高翔博士的《视觉slam十四讲从理论到实践》或者自行搜索。下面这张图也是来自这本书的:
这张图给出了李群和李代数之间的转换公式。有了这个转换,离我们求解1式问题还有些距离。在使用类似高斯牛顿法这样的迭代算法时,目标函数关于变量的导数很重要,因为它提供了目标函数变小的方向。其实就是变量选择的方向,那么我们就想知道目标函数关于李代数的导数。
使用李代数解决问题的求导思路分为两种:
1. 用李代数表示位姿,然后根据李代数加法来对李代数求导
2. 对李群左乘或者右乘微小扰动量,然后对该扰动求导,成为左扰动和右扰动模型。
我们先说关于旋转矩阵对应的李代数 SO(3) 求导,第一种的求导过程,如下:
由于旋转矩阵对应的李代数是一个三维向量,所以我们可以用模长与单位向量的乘积表示。 ϕ=θa , θ为模长,a为单位向量 ,上式中 θ 和 a 就是这个含义。
下来我们在来看看旋转矩阵李代数的左扰动模型,左乘一个微小扰动,对微小扰动求导:
这种方式省去了计算雅克比 Jl ,所以使用更为常见。
最后直接给出SE(3)上的李代数求导,使用左扰动模型:
有了李群和李代数相互转换公式,李代数上的导数,那么我们就可以将1式用李代数重新表达。
上式仍然只是一个粗略的表达,其中没有显式说明齐次与非齐次,我们认为自动满足。使用李代数我们可以成功的将位姿约束问题转化为无约束问题,但要用高斯牛顿法或者LM方法等迭代优化算法,我们还需要知道这个误差关于位姿李代数的导数。使用左扰动模型,利用链式法则展开:
取出这部分的前三行,然后两个导数项相乘,就可以得到一个2*6的雅克比矩阵:
如果变换矩se(3)的旋转在前,平移在后,只需要将上式的前面3列和后面3列对调即可。有了李代数表示,也有了李代数上的表示,下一步我们就可以利用一些优化库(ceres, g2o)去计算了。
[1] 高博, 视觉slam十四讲从理论到实践