《视觉SLAM十四讲》学习笔记-Bundle Adjustment (BA) 问题

• BA本质是把PnP问题当成李代数上的非线性最小二乘问题。
• 线性问题的方式：先求相机位姿，再求空间点位置

问题：给定$n$个三维空间点$P$和投影$p$，希望计算相机的位姿$\mathbf{R}$和$\vec{t}$,对应的李代数为$\xi$. 假设：$\mathbf{P}_i=[X_i, Y_i, Z_i]^T$,则像素位置与空间位置的关系为：

$$s \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} =\mathbf{K}\exp(\xi^\wedge) \begin{bmatrix} X_i \\ Y_i \\ Z_i\\ 1 \end{bmatrix}$$
写在矩阵形式为：
$$s_i\vec{u}_i=\mathbf{K}\exp(\xi^\wedge)\mathbf{P}_i$$
上式中隐含有一个 齐次坐标到非齐次坐标的转换。把误差求和，构建最小二乘问题，寻找最好的相机位姿：
$$\xi^* = \arg \underset{\xi}{\min} \frac{1}{2} \sum_{i=1}^{n} | \vec{u}_i - \frac{1}{s_i}\mathbf{K}\exp(\xi^\wedge)\mathbf{P}_i |_2^2$$
上式为重投影误差.
应用G-N或L-M等方法之前，要知道误差项关于变量的导数，即：
$$e(\vec{x}+\triangle \vec{x}) \approx e(x)+\mathbf{J}\triangle x$$
这里：
* $e$的意义为像素坐标误差，为2维
* $x$为相机位姿，6维变量
* $\mathbf{J}$为 $2\times 6$的矩阵，需要进一步推导。

设变换到相机坐标系下的空间点坐标为$\mathbf{P}'$：

$$\mathbf{P}' = \left(\exp(\xi^\wedge)\mathbf{P}\right)_{1:3} = [X', Y', Z']^\top$$
则相机投影模型为：
$$s\vec{u}=\mathbf{K}\mathbf{P}'$$
即：
$$\begin{bmatrix} su \\sv \\ s \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X' & Y' & Z' \end{bmatrix}$$
所以：
$$u = f_x\frac{X'}{Z'} + c_x, ~~~ v=f_y\frac{Y'}{Z'} + c_y$$
考虑对 $\xi^\wedge$的左乘扰动量 $\delta\xi$关于 $e$的变化，根据链式法则有：

$$\frac{\partial e}{\partial \delta \xi} = \lim\limits_{\delta \xi \rightarrow 0} \frac{e\left( \delta\xi\oplus \xi\right) }{\delta\xi} = \frac{\partial e}{\partial \mathbf{P}'}\frac{\partial \mathbf{P}'}{\partial \delta \xi}$$

其中符号$\oplus$为李代数上的左乘扰动。所以有：

$$\frac{\partial e}{\partial \mathbf{P}'} = - \begin{bmatrix} \frac{\partial u}{\partial X'} & \frac{\partial u}{\partial Y'} & \frac{\partial u}{\partial Z'} \\ \frac{\partial v}{\partial X'} & \frac{\partial v}{\partial Y'} & \frac{\partial v}{\partial Z'} \end{bmatrix} = - \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2}\\ 0 & \frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} \end{bmatrix}$$
式 $\eqref{eq:e_delta}$的第二项为变换后的点关于李代数的导数：
$$\frac{\partial\mathbf{TP} }{\partial \delta \xi} = (\mathbf{TP})^\odot = \begin{bmatrix} \mathbf{I} & -\mathbf{P}'^\wedge\\ 0^\top & 0^\top \end{bmatrix}$$
又由于：
$$\frac{\partial \mathbf{P}'}{\partial \delta \xi} =[\mathbf{I}, -\mathbf{P}'^\wedge]$$
将这两项相乘后雅可比矩阵为：
$$\frac{\partial e}{\partial \delta \xi} = - \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{f_x X'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2} & f_x + \frac{f_x X^2}{Z'^2} & -\frac{f_x Y'}{Z'}\\ 0 & \frac{f_y}{Z'} & -\frac{f_y Y'}{Z'^2} & -f_y -\frac{f_y Y'^2}{Z'^2} & \frac{f_yX'Y'}{Z'^2} & \frac{f_yX'}{Z'} \end{bmatrix}$$
该矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系.

另一方面，除了要优化位姿，也得优化特征点的空间位置，即需要讨论$e$关于空间点$\mathbf{P}$的导数。
利用链式法则，有：

$$\frac{\partial e}{\partial \mathbf{P} } = \frac{\partial e}{\partial \mathbf{P}'} \frac{\partial \mathbf{P}'}{\partial \mathbf{P}}$$

第一项与前面的式子相同。第二项按照定义有：

$$\mathbf{P}' = \exp\left( \xi^\wedge \right) \mathbf{P} = \mathbf{RP}+\vec{t}$$
对 $\mathbf{P}$求导后只剩下 $\mathbf{R}$,所以：
$$\frac{\partial e}{\partial\mathbf{P}} = - \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2}\\ 0 & \frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} \end{bmatrix}\mathbf{R}$$
上式即为 相机方程关于相机特征点的导数矩阵方程。