李群李代数及BA优化的一点理解

概要

咸鱼了好久,为了个1024徽章还是上传点什么吧…
SLAM知识点很多,李群李代数和BA优化是其中比较重要的点. 本文主要是将<<视觉SLAM14讲>>中的一些相关内容按自己的思路整理了一下.

李群李代数

从实用角度讲,李群李代数主要是讲旋转矩阵或变换矩阵与向量之间的映射关系,只要理解了下面两张图以及扰动方程即可. 以旋转矩阵R为例,它组成了李群SO(3).而旋转矩阵由与三个坐标轴的夹角确定,所以有三个自由度,因此旋转矩阵可由一个三维向量$\varphi$表示,此三维向量称为旋转矩阵的旋转向量,此旋转向量组成的空间也就是李群SO(3)对应的李代数小so(3),也就是$R=\exp (\varphi \hat{})$,其中$\hat{}$表示向量的反对称矩阵或称向量的叉乘矩阵,更具体映射关系如下图:

其中:$\theta$为$\varphi$的向量长度, $a$为$\varphi$的方向向量, 同样的由变换矩阵T组成的李群SE(3)与6维向量组成的李代数se(3)的映射关系如下:

李群李代数的出现是为了解决旋转矩阵或变换矩阵的求导问题: 以李群SE(3)为列,假设我们对一个空间点进行旋转和平移,得到了Tp.要计算变换之后的坐标相对于变换矩阵的导数,我们不严谨地记为.
$$\frac{\partial Tp}{\partial T}$$
因为变换矩阵T对乘法封闭,对加法不封闭.即变换矩阵乘以变换矩阵还是变换矩阵,但变换矩阵加上变换矩阵不一定还是变换矩阵,所以无法用导数的定义进行求导,因此需要转成李代数(李代数由向量组成,显然对加法封闭),如下:
$$\frac{\partial Tp}{\partial T}=\frac{\partial (exp(\xi \hat{}p))}{\partial \xi }$$
所谓扰动模型是指另一种求导方式,比如左振动模型就是在左乘一个$\Delta T=\exp (\delta \xi )$如:

$$\begin{gathered} \frac{\partial Tp}{\partial T}=\underset{\Delta T\to 0}{\lim }\frac{\Delta T\cdot Tp-Tp}{\Delta T} \\ =\underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta \xi \hat{})\exp (\xi \hat{})p-\exp (\xi \hat{})p}{\delta \xi } \end{gathered}$$
化简后最终可得:
$$\frac{\partial Tp}{\partial T}=\left[\begin{array}{cc}I& -(Rp+t)\hat{}\\ 0^T& 0^T\end{array}\right]$$
实际应用中,变换矩阵的求导都是利用振动方程, 例如BA求解时雅可比矩阵J的计算.

从实际应用了解Bundle Adjustment(BA)

我们直接从实际应用来了解Bundle Adjustment,首先定义一个通用观测方程为:
$$u=h(\xi ,p)$$
其中$\xi$为变换矩阵T对应的李代数,指代此时相机的位姿, p 为各个路标点的三维点坐标,而观测数据u则是像素坐标。以最小二乘的角度来考虑,那么可以列写关于此次观测的误差:
$$e=u-h(\xi ,p)$$
然后,把其他时刻的观测量也考虑进来:设$u_{ij}$为在位姿${\xi }_i$ 处观察路标$p_j$产生的数据,那么整体的代价函数为:
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}_2^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||u_{ij}-h({\xi }_i,p_j)|{|}_2^2(1)$$
对这个最小二乘进行求解,即寻找最好的相机位姿和路标点使误差最小,相当于对位姿和路标同时作了调整,也就是所谓的 BA, 上式的求解在SLAM中属于全局BA, 如果只考虑两帧的时间,这个问题就可以当成PNP问题,也就是局部BA.

BA求解思路

在上面的观测方程中,我们把自变量定义成所有待优化的变量,即令:
$$\begin{gathered} x=[{\xi }_1,...,{\xi }_m,p_1,...,p_n{]}^T \\ e=f(x) \end{gathered}$$
则式(1)BA问题可以看成一个基本的最小二乘问题:
$$x=\arg \underset{x}{\min }\frac{1}{2}||f(x)|{|}_2^2$$
大部分情况下,$f$较为复杂,不方便使用求导法直接求解,而是采取迭代的方法,类似于梯度下降. 这里介绍使用Gauss-Newton方法:
首先将$f(x)$一阶泰勒展开:
$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$
注意这时$f$与$x$都是多元的,因此J(x)实际上是m*n的矩阵,也就是一个雅可比矩阵(一阶偏导数以一定方式排列成的矩阵).
当前的目标是为了寻找下降矢量 $\Delta x$,使得$||f(x+\Delta x)|{|}^2$ 达到最小。为了求 ∆x,我们需要解一个线性的最小二乘问题:
$$\Delta x^{\ast }=\arg \underset{\Delta x}{\min }||f(x)+J(x)\Delta x)|{|}_2^2$$
将上述目标函数$||f(x)+J(x)\Delta x)|{|}^2$对$\Delta x$求导,并令导数为零,省略计算过程 ,可得如下方程组:
$$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x)$$
我们将此方程称为增量方程,通过它可以直接求解$\Delta x$,因此Gauss-Newton 的算法步骤可以写成:

1. 给定初始值 $x_0$ 。

2. 对于第 k 次迭代,求出当前的雅可比矩阵 $J(x_k)$和误差 $f(x_k)$。

3. 求解增量方程: $J(x_k{)}^{T}J(x_k)\Delta x_k=-J(x_k{)}^{T}f(x_k)$.

4. 若 $\Delta x_k$足够小,则停止。否则,令 $x_{k+1}=x_k+\Delta x_k$,返回 2.

因此问题的关键在于求解雅可比矩阵 $J(x)$, 因为x由各时刻的$\xi$和p组成, 易得:
$$||f(x+\Delta x)|{|}_2^2=||f(x)+F\Delta \xi +E\Delta p|{|}_2^2$$
其中F,E分别为f(x)关于$\xi$和p的偏导,由分块矩阵, 雅可比矩阵J可由F和E拼凑起来, 即$J=[FE]$. 由相机模型和振动方程可得(视觉SLAM14讲7.73):
$$F=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y+\frac{f_y{Y}^2}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& -\frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

$$E=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R$$

其中$f_x,f_y$指的是相机内参的参数而不是偏导.R为旋转矩阵, $[X^{\prime },Y^{\prime },Z^{\prime }]$为点P在相机坐标系下的坐标,即
$$[X^{\prime },Y^{\prime },Z^{\prime }]=TP$$
至此, Gauss-Newton 的算法步骤中的每一步都可计算,因此就可以计算出BA的解了. 当然实际求解时,由于雅可比矩阵J过大, 需要利用矩阵的稀疏性和边缘性来加速运算,这时就要用到g2o 和 Ceres 两个库.