为了解决的问题:
假设某个时刻相机的位姿是T,它观察到一个在世界坐标系中的一个空间点p,并在相机上产生了一个观测数据z,那么
z = Tp + noise
noise是观测噪声。那么观测误差就是
e = z - Tp
假设我们总共有N个这样的三维点p和观测值z,那么我们的目标就是寻找一个最佳的位姿T,使得整体误差最小化,也就是
求解此问题,也就是求目标函数J对于变换矩阵T的导数,而T所在的的SE(3)空间,对加法计算并不封闭,也就是说任意两个变换矩阵相加后并不是一个变换矩阵,这主要是因为旋转矩阵对加法是不封闭造成的,它是有如下约束:
李代数就是解决旋转矩阵对加法不封闭的问题。我们把大写SE(3)空间的T映射为一种叫做李代数的东西,映射后的李代数我们叫做小se(3)好了。它是由向量组成的,我们知道向量是对加法封闭的。这样我们就可以通过对李代数求导来间接的对变换矩阵求导了。
我们SLAM目的就是优化求解相机的这个最佳的位姿T(变换矩阵),优化方法一般都采用迭代优化的方法,每次迭代都更新一个位姿的增量delta,使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分(导数),
李群
群(group)就是一种集合加上一种运算的代数结构,满足封闭性,结合律,幺元(单位矩阵I)和逆。李群的定义是指连续光滑的群,比如我们前面说的旋转矩阵群SO(3),你想象你拿个杯子就可以在空间中以某个支点连续的旋转它,所以SO(3)它就是李群。如果你一般旋转一边移动它,也是连续的或者说光滑的运动,所以变换矩阵群SE(3)也是李群。
李代数
李代数对应李群的正切空间,它描述了李群局部的导数。
对于某个时刻的R(t)(李群空间),存在一个三维向量φ=(φ1,φ2,φ3)(李代数空间),用来描述R在t时刻的局部的导数。
我们发现旋转矩阵的微分是一个反对称(也叫斜对称)矩阵左乘它本身,反对称矩阵其实是将三维向量和三维矩阵建立对应关系。它是这样定义的:如果一个3 X 3的矩阵A满足如下式子,也就是说反对称矩阵对角线元素都为0。
对于反对称矩阵A,只有三个自由度:
则定义一个三维向量$a=\left[ a_{1}\; ,\; a_{2}\; ,\; a_{3}\; \right]^{T}$,则可以用一个上三角符号来定义如下对应关系:
这样就可以顺利理解
指数映射关系
李代数小so(3)是三维向量φ的集合,每个向量φi的反对称矩阵都可以表达李群(大SO(3))上旋转矩阵R的导数,而R和φ是一个指数映射关系。我们的目的就是用对李代数求导来间接的对变换矩阵求导了,也就是说,李群空间的任意一个旋转矩阵R都可以用李代数空间的一个向量的反对称矩阵指数来近似。
我们最终得到下面式子,它的前提是R在原点附近的一阶泰勒展开,我们看到这个向量φ=(φ1,φ2,φ3)反应了R的导数性质,故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。
经过对指数e的泰勒展开,以及反对称矩阵的性质,我们可以得到如下结果:
其中:三维向量 φ = θa,a是一个长度为1的方向向量,其实有点像罗德里格斯公式 ,罗德里格斯公式(Rodriguez formula)是计算机视觉中的一大经典公式,在描述相机位姿的过程中很常用,表示从旋转向量到旋转矩阵的转换过程,公式如下
其中:$k=\left[ k_{x}\; k_{y}\; k_{z} \right]^{T}$是旋转轴(两个自由度),theta是旋转角度(一个自由度);
综上所述,我们得到如下映射关系
李代数求导分两种:一种是用李代数表示位姿,然后根据李代数加法来对李代数求导,但结果中有复杂的雅克比公式,不是很方便。一般都用第二种,就是对李群进行左乘或者右乘微小的扰动,然后对该扰动求导。