Bundle Adjustment 求解 主页                                            博客   留言 

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

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

\[p' = Rp + t = [x', y', z']. \tag{1.1}\]

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

\[p_c = [u_c, v_c, 1]^T = [x'/z', y'/z', 1]^T \tag{1.2}\]

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

\[u_s = f_xu_c + c_x \\ \tag{1.3} v_s = f_yv_c + c_y\]

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

\[p_s = \frac{Kexp(\zeta)p}{z'} \\ \tag{1.4} p_s = h(\zeta, p)\]

其中(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]$, 我们以最小二乘的角度来考虑,定义观测值与估计值之间的代价函数:

\[e = z - p_s = z - h(\zeta, p). \tag{1.5}\]

其中$p_s$ 为通过当前的$\zeta, p$ 估计出来的$p$的像素坐标,而$z$为观测到的像素坐标。通过考虑其他时刻的观测量,我们可以给误差添加一个下标:设$z_{ij}$为在位姿$\zeta_i$处观察路标$p_j$产生的数据,那么整体的代价函数为: \(\frac{1}{2}\sum_{i=1}^m\sum_{j=1}^n||e_{ij}||^2=\frac{1}{2}\sum_{i=1}^m\sum_{j=1}^n||z_{ij}-h(\zeta_i, p_j)||^2. \tag{1.6}\)

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

2.高斯牛顿法

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

\[min_{x}\frac{1}{2}||f(x)||_2^2 \tag{2.1}\]

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

\[f(x + \Delta x )=f(x) + J(x)\Delta x \tag{2.2}\]

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

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

\[\frac{1}{2} ||f(x) + J(x)\Delta x ||^2 = \frac{1}{2}(f(x)+J(x)\Delta x)^T(f(x) + J(x)\Delta x) \\ = \frac{1}{2}(||f(x)||^2_2 + 2f(x)^TJ(x)\Delta x + \Delta x^TJ(x)^TJ(x)\Delta x).\]

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

\[2J(x)^Tf(x) + 2J(x)^TJ(x)\Delta x = 0. \tag{2.4}\]

可以得到下列方程组:

\[J(x)^TJ(x)\Delta x = -J(x)^Tf(x). \tag{2.5}\]

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

\[H\Delta x = g. \tag{2.6}\]

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

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

\[min_x \frac{1}{2}\sum_{i=1}^{m*n}||f_i(x)||^2_2 \tag{2.7}\]

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

\[\sum_{i=1}^{m*n}J_i(x)^TJ_i(x)\Delta x=\sum_{i=1}^{m*n}-J_i(x)^Tf_i(x) \tag{2.8}\]

我们令\(H = \sum_{i=1}^{m*n}J_i(x)^TJ_i(x),g = \sum_{i=1}^{m*n}-J_i(x)^Tf_i(x)\), 同理可以得到:

\[H\Delta x = g. \tag{2.9}\]

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

3. 雅可比矩阵的推导

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

\[x = [\zeta_1, ..., \zeta_m, p_1, ..., p_n]\]

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

\[H = \sum_{i=1}^m\sum_{j=1}^n J_{ij}(x)^TJ_{ij}(x)\]

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

\[J_{ij}=\frac{\partial e_{ij}}{\partial x} = (0_{2\times 6}, ..., \frac{\partial e_{ij}}{\partial \zeta_i}, ..., 0_{2\times 6}, 0_{2\times 3}, ..., \frac{\partial e_{ij}}{\partial p_j}, ...,0_{2\times 3}).\]

其中$\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$,利用链式法则,可以列写如下:

\[\frac{\partial e_{ij}}{\partial \delta\zeta_{i}} = lim_{\partial \zeta->0}\frac{e(\delta \zeta \oplus \zeta)}{\delta \zeta} = \frac{\partial e_{ij}}{\partial p'_j} \frac{\partial p_j'}{\partial \delta \zeta_i}\]

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

\[\frac{\partial e_{ij}}{\partial p_j'} = - \left[ \begin{matrix} \frac{f_x}{z_j'} & 0 & -\frac{f_x x_j'}{z_j'^2} \\ 0 & \frac{f_y}{z'_j} & -\frac{f_yy_j'}{z_j'^2} \end{matrix} \right]\]

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

\[\frac{\partial p_j'}{\partial \delta \zeta_i} = \frac{\partial Tp_j}{\partial\delta\zeta_i} = (Tp_j)^{\odot}=[I, -p_j'^\wedge]\]

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

\[\frac{\partial e_{ij}}{\partial \delta\zeta_{i}} = - \left[ \begin{matrix} \frac{f_x}{z_j'} & 0 & -\frac{f_x x_j'}{z_j'^2} & -\frac{f_xx_j'y_j'}{z_j'^2} & f_x + \frac{f_x x_j'^2}{z_j'^2} & -\frac{f_x y_j'}{z_j'}\\ 0 & \frac{f_y}{z'_j} & -\frac{f_yy_j'}{z_j'^2} & -f_y-\frac{f_y y_j'^2}{z_j'^2} & \frac{f_y x_j'y_j'}{z_j'^2} & \frac{f_yx_j'}{z_j'} \end{matrix} \right]\]

其中\(x'_j, y_j', z_j'\) 分别为在姿态 $\zeta_i$ 下 \(x_j, y_j, z_j\) 的投影。 另一方面,对于\(\frac{\partial e_{ij}}{\partial p_j}\): \(\frac{\partial e_{ij}}{\partial p_j} = \frac{\partial e_{ij}}{\partial p'_j}\frac{\partial p'_j}{\partial p_j}\)

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

\[\frac{\partial e_{ij}}{\partial p_j} =- \left[ \begin{matrix} \frac{f_x}{z_j'} & 0 & -\frac{f_x x_j'}{z_j'^2} \\ 0 & \frac{f_y}{z'_j} & -\frac{f_yy_j'}{z_j'^2} \end{matrix} \right] R_i\]

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