采用逆深度参数表达的BA问题导数推导

采用逆深度参数表达的BA问题导数推导

---

由于大部分的slam算法均采用逆深度参数表达地图点的结构，但是网上对该方法的介绍比较少因此本文将详细说明其推导过程。逆深度参数表达具有优化变量少、能表达非常远的点以及分布接近高斯分布等优势，这也是大家选择逆深度参数的原因。

---

图结构表达

从上图可以看出一个约束项$E_{p_{12}}$连接三个节点分别是当前帧、参考帧和逆深度参数表达的地图点。这是因为在逆深度参数的表达下需要当前帧、参考帧位姿及地图点才能共同确定误差函数，这将在下文中看到。

优化节点

优化节点分为两类，关键帧的相机位姿和地图点坐标。其中关键帧的相机位姿维护SE(3)表达的状态，而地图点则是逆深度参数。

相机位姿

状态量采用四元数+平移向量的方式表达，即[q|t],对于微小的角度更新方法如下：
$$\begin{gathered} t_{k+1}=t_k+{\delta }_t \\ {q}_{k+1}=q_k\oplus {\delta }_q \end{gathered}$$
其中$\oplus$代表的是四元数更新，一般采用四阶Runge-Kunta法更新四元数。

$$\frac{\partial p_f}{\partial {\delta }_{\xi }}=(TP{)}^{\odot }=\left[\begin{array}{cc}I& -p_f^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
其中$p_f$为当前坐标系下的$XYZ$表达地图点，导数的具体求解过程参考附录。

逆深度参数地图点

逆深度参数采用MSCKF表示：

$$p_f=\left[\begin{array}{c}{x}_f\\ y_f\\ z_f\end{array}\right]=\frac{1}{\rho }\left[\begin{array}{c}\alpha \\ \beta \\ 1\end{array}\right]$$
$$\lambda =\left[\begin{array}{c}\alpha \\ \beta \\ \rho \end{array}\right]=\left[\begin{array}{c}{x}_f/z_f\\ y_f/z_f\\ 1/z_f\end{array}\right]$$
$$\frac{\partial p_f}{\partial \lambda }=\left[\begin{array}{ccc}\frac{1}{\rho }& 0& -\frac{\alpha }{{\rho }^2}\\ 0& \frac{1}{\rho }& -\frac{\beta }{{\rho }^2}\\ 0& 0& -\frac{1}{{\rho }^2}\end{array}\right]$$
其中$p_f$为参考坐标系下地图点的三维坐标，$x_f,y_f,z_f$分别为对应数值，$\lambda$为逆深度参数表达的坐标而二者的导数关系也一并给出。推导公式参考附录。

边约束项

误差计算的思路在于把参考帧下的地图点转换到当前帧坐标系下，在投影到当前帧像素平面上与对应的特征点坐标求差值。因此误差函数为：
$$e=\left[\begin{array}{c}{u}_m\\ v_m\end{array}\right]-\left[\begin{array}{c}u\\ v\end{array}\right]$$
$$\left[\begin{array}{c}{u}_m\\ v_m\end{array}\right]=H(T_{wc}^{-1}{T}_{wf}G(\lambda ))$$
其中$\left[\begin{array}{c}{u}_m\\ v_m\end{array}\right]$是重投影的点，$H$函数根据相机模型将相机坐标系下的点投影到像素坐标系中，而$G$函数则将逆深度参数转换为参考帧下的3d坐标，对于$T_{wc},T_{wf}$则分别是世界系下当前相机帧的位姿和参考帧的相机位姿。

更进一步，将误差函数展开成为更加具体的形式。

$$\left[\begin{array}{c}{u}_m\\ v_m\end{array}\right]=\left[\begin{array}{c}f\frac{x_c}{z_c}+c_x\\ f\frac{y_c}{z_c}+c_y\end{array}\right]$$

$$p_c=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R_{wc}^{-1}{p}_w-R_{wc}^{-1}{t}_{wc}$$

$$p_w=R_{wf}{p}_f+t_{wf}$$

$$p_f=\frac{1}{\rho }\left[\begin{array}{c}\alpha \\ \beta \\ 1\end{array}\right]$$

将误差函数通过一步步转换成为逆深度参数坐标，在求导时便可轻易的采用链式法则进行计算。

对地图点的导数

$$\frac{\partial e}{\partial \lambda }=\frac{\partial e}{\partial p_c}\frac{\partial p_c}{\partial p_f}\frac{\partial p_f}{\partial \lambda }$$

由于$\frac{\partial p_f}{\partial \lambda }$已知，因此只需要求解$\frac{\partial e}{\partial p_c},\frac{\partial p_c}{\partial p_f}$

$$\frac{\partial e}{\partial p_c}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial x_c}& \frac{\partial u}{\partial y_c}& \frac{\partial u}{\partial z_c}\\ \frac{\partial v}{\partial x_c}& \frac{\partial v}{\partial y_c}& \frac{\partial v}{\partial z_c}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f}{z_c}& 0& -\frac{f{x}_c}{z_c^2}\\ 0& \frac{f}{z_c}& -\frac{f{y}_c}{z_c^2}\end{array}\right]$$

$$\frac{\partial p_c}{\partial p_f}=R_{cf}$$

对当前帧的导数

$$\frac{\partial e}{\partial {\delta }_{{\xi }_c}}=\frac{\partial e}{\partial p_c}\frac{\partial p_c}{\partial {\delta }_{{\xi }_c}}$$
其中，$\frac{\partial e}{\partial p_c}$在对点求导时已求得，而$\frac{\partial p_c}{\partial {\delta }_{{\xi }_c}}$只要带入可得
$$\frac{\partial p_c}{\partial {\delta }_{{\xi }_c}}=\left[\begin{array}{cc}I& -p_c^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
在计算时通常会直接删除下面为零的部分，以便和前面矩阵维度一致。

对参考帧的导数

$$\frac{\partial e}{\partial {\delta }_{{\xi }_f}}=\frac{\partial e}{\partial p_f}\frac{\partial p_f}{\partial {\delta }_{{\xi }_f}}$$
同样，$\frac{\partial e}{\partial p_f}$在对点求导时已经求得了
$$\frac{\partial p_c}{\partial {\delta }_{{\xi }_f}}=\left[\begin{array}{cc}I& -p_f^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

附录

向量的求导

$$\frac{\partial \left[\begin{array}{c}a\\ b\end{array}\right]}{\partial \left[\begin{array}{c}x\\ y\end{array}\right]}=\left[\begin{array}{cc}\frac{\partial a}{\partial x}& \frac{\partial a}{\partial y}\\ \frac{\partial b}{\partial x}& \frac{\partial b}{\partial y}\end{array}\right]$$

对微小的扰动${\delta }_{\xi }$求导