1. 相机模型与BA代价函数

我们首先回顾一下三维坐标投影到像素空间的过程。我们从世界坐标系中的点$p$出发。首先,我们将世界坐标转换到相机坐标,这里我们要用到相机外参数$(R, t)$:

接着,我们将$p’$ 投影到归一化平面, 得到归一化坐标:

最后,根据内参模型,我们计算像素坐标:

我们令$p_s = [u_s, v_s]^T$, 将上述的过程转换成矩阵和函数形式可表示为:

其中(1.4)隐含了一次其次坐标到非齐次坐标的转换。$\zeta \in R^6$为相机当前的位姿的李代数,$p \in R^3$为当前相机的三维坐标。$\zeta, p$为我们待估计的状态。

所谓BA(Bundle Adjustment), 其实指通过将$\zeta, p$作为优化变量,并最小化估计值与观测值的代价函数,来达到估计$\zeta, p$的目的。而观测数据则是观测到的$p$的像素坐标$z = [u_z, v_z]$, 我们以最小二乘的角度来考虑,定义观测值与估计值之间的代价函数:

其中$p_s$ 为通过当前的$\zeta, p$ 估计出来的$p$的像素坐标,而$z$为观测到的像素坐标。通过考虑其他时刻的观测量,我们可以给误差添加一个下标:设$z_{ij}$为在位姿$\zeta_i$处观察路标$p_j$产生的数据,那么整体的代价函数为:

对这个最小二乘进行求解,相当于对位姿和路标同时进行了调整。为了优化这个代价函数,我们需要使用高斯牛顿法或者列文伯格-马夸尔特方法来进行优化。

2.高斯牛顿法

我们首先考虑一个最简单的最小二乘问题:

我们假设它有$m$ 维,即$f(x) \in R^m, x \in R^n$。我们首先将$f(x)$ 进行一阶的泰勒展开:

其中$J(x) \in R^{m \times n}$为$f(x)$ 关于$x$ 的导数, 也成为雅可比矩阵。

我们当前的目标为寻找下降矢量$\Delta x$ ,使式(2.2)最小。因此将(2.2)式带入可得:

求上述式子关于$\Delta x$的导数,并令其为零。

可以得到下列方程组:

注意我们要求解的是$\Delta x$, 因此这是一个线性方程组,我们称它为增量方程。我们把左边的系数定位$H$, 右边的系数定为$g$, 那么上述式子变为:

对比牛顿法可见,高斯牛顿法使用$J^TJ$作为牛顿法中二阶$H$矩阵的近似。从而省略了计算$H$的过程。

对于多个$f$的最小二乘问题,形式如下:

这个形式对应于式(1.6),并通过上述类似的推导,可以得到:

我们令, 同理可以得到:

通过上述推导,我们便可以开始着手解决BA问题。我们首先要推导出$H$的形式。

3. 雅可比矩阵的推导

我们首先统一待优化的所有变量:

同时, 令$e_{ij}$ 对应$x$的雅可比矩阵为$J_{ij}(x)$。则对应的$H$矩阵为:

因此我们需要推导出$J_{ij}$的具体形式。由于$e_{ij}$只与$\zeta_i, p_j$有关,因此关于其他优化变量的导数都为零。我们可以得到:

其中$\frac{\partial e_{ij}}{\partial \zeta_i} \in R^{2 \times 6}, \frac{\partial e_{ij}}{\partial p_j} \in R^{2\times 3}$.接下来我们要推导这两个表达式的具体形式。

我们首先对$\zeta $左乘扰动量$\delta \zeta$,利用链式法则,可以列写如下:

这里的$\oplus$指李代数上的左乘扰动,第一项是误差关于投影点的导数,由式(1.1-1.5)易得:

而第二项为变换后的点关于李代数的导数,由左乘扰动求导的公式可得:

将上述两个式子相乘,即得到了雅可比矩阵$\frac{\partial e_{ij}}{\partial \delta\zeta_{i}}$.

其中 分别为在姿态 $\zeta_i$ 下 的投影。 另一方面,对于:

其中第一项已经推导,第二项推导完只剩下旋转矩阵,因此可得:

其中$R_i$ 为$\zeta_i$下的旋转矩阵。至此我们$J_{ij}$的表达式和$H$的表达式推导完毕。