採用逆深度參數表達的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}$求導