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

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

---

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

---

图结构表达

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

优化节点

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

相机位姿

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

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

逆深度参数地图点

逆深度参数采用MSCKF表示：

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

边约束项

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

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

$\left[\begin{matrix} u_m \\ v_m \end{matrix}\right] = \left[\begin{matrix} f \frac{x_c}{z_c} + c_x \\ f \frac{y_c}{z_c} + c_y \end{matrix}\right]$

$p_c=\left[\begin{matrix} x_c \\ y_c \\ z_c \end{matrix}\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{matrix} \alpha \\ \beta \\ 1 \end{matrix}\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{matrix} \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{matrix}\right] = - \left[\begin{matrix} \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{matrix}\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{matrix} I & -p_c^{\wedge} \\ 0^T & 0^T \end{matrix}\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{matrix} I & -p_f^{\wedge} \\ 0^T & 0^T \end{matrix}\right]$

附录

向量的求导

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

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