視覺SLAM十四講 第七講 視覺里程計1 Bundle Adjustment公式推導

PnP問題可構建成一個定義於李代數上的非線性最小二乘問題。
線性方法往往是先求位姿，再求空間點位置。非線性優化把它們都看成優化變量，放在一起優化。在PnP中，這個Bundle Adjustment問題，是一個最小化重投影誤差（Reprojection error)的問題。
n個三維空間點P及其投影p，希望計算相機的位姿 $\boldsymbol{R,t}$ ，它的李代數爲 $\boldsymbol\xi$ 。假設某空間點座標爲 $P_i=[X_i,Y_i,Z_i]^T$ ,投影像素座標爲 $\boldsymbol{u_i}=[u_i,v_i]$ 。像素位置於空間點位置關係如下：
$s_i\left[ \begin{matrix}u_i\\v_i\\1\end{matrix}\right]=Kexp(\xi^{\wedge})\left[ \begin{matrix}X_i\\Y_i\\Z_i\\1\end{matrix}\right]$
除了用 $\xi$表示相機位姿之外，別的都和之前的定義保持一致。寫成矩陣形式就是：
$s_iu_i=Kexp(\xi^{\wedge})P_i$
中間隱含齊次座標到非齊次座標的轉換。由於相機位姿未知及觀測點的噪聲，該等式存在一個誤差，因此，把誤差求和，構建最小二乘問題，然後尋找最好的相機位姿，使它最小化：
$\xi^{*}=arg\min \limits_{\xi} \frac{1}{2}\sum_{i=1}^n\left \| u_i-\frac{1}{s_i}Kexp(\xi^{\wedge})P_i\right\|_2^2$
該問題的誤差項，是將像素座標（觀測到的投影位置）於3D點安裝當前估計的位姿進行投影得到的位置相比較得到的誤差，作以稱爲重投影誤差。使用齊次座標時，這個座標有3維。不過，由於 $\boldsymbol{u}$ 最後一維爲1，該維度的誤差一直爲零，因而使用非齊次座標，於是誤差就只有兩維了。

通過特徵匹配知道了 $p_1,p_2$ 是同一個空間點P的投影，但不知道相機的位姿。在初始值中，P的投影 $\hat{p}_2$ 與實際的 $p_2$ 之間有一定的距離。於是調整相機的位姿，使得這個距離變小。由於調整需要考慮很多個點，最好每個點的誤差通常不會精確爲零。
每個誤差項關於優化變量的導數，線性化：
$e(x+\Delta{x})\approx e(x)+J\Delta{x}$
當 e 爲像素座標（2維）， x 爲相機座標(6維)時， J 是一個 $2\times6$ 的矩陣。J 的形式推導如下：
變換到相機座標系下的空間點座標爲$P^{'}$ ，取出前三維：
$P'=(exp(\xi^{\wedge})P)_{1:3}=[X',Y',Z']^T$
相機投影模型相對於 P’ 爲
$s\boldsymbol u=KP'$
展開：
$\left[ \begin{matrix}su\\sv\\s\end{matrix}\right]=\left[ \begin{matrix}fx&0&c_x\\0&f_y&c_y\\0&0&1\end{matrix}\right]\left[\begin{matrix}X'\\Y'\\Z'\end{matrix}\right]$
利用第三行消去s（實際上就是 P’ 的距離），得：
$u=f_x\frac{X'}{Z'}+c_x,v=f_y\frac{X'}{Z'}+c_y$
求誤差時，可以把這裏的u,v與實際的測量值進行比較，求差。定義了中間變量後對 $\xi^{\wedge}$ 左乘擾動量 $\delta \xi$ ，然後考慮 e 的變化關於擾動量的導數。利用鏈式法則：
$\frac{\partial e}{\partial \delta\xi}=\lim_{\partial \xi \to 0}\frac{e(\delta \xi\oplus\xi)}{\delta \xi}=\frac{\partial e}{\partial P'}\frac{\partial P'}{\partial \delta \xi}$
$\oplus$ 指李代數上的左乘擾動。第一項是誤差關於投影點的導數：
$\frac{\partial e}{\partial P'}=-\left[ \begin{matrix}\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{matrix}\right]=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]$
第二項爲變換到相機座標系下的空間點 P’ 關於李代數的導數
$\frac{\partial(TP)}{\partial \delta \xi}=(TP)^\odot=\left[ \begin{matrix}I&-P'^{\wedge}\\0^T&0^T\end{matrix}\right]$
在 P’ 的定義中，去除前三維得：
$\frac{\partial P'}{\partial \delta \xi}=\left[ \begin{matrix}I&-P'^{\wedge}\end{matrix}\right]$
將這兩項相乘，就得到了 2\times6 的雅可比矩陣：
$\frac{\partial e}{\partial \delta\xi}=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}&-\frac{f_xX'Y'}{Z'^2}&f_x+\frac{f_xX'^2}{Z'^2}&-\frac{f_xY'}{Z'}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'}\end{matrix}\right]$
這個雅可比矩陣描述了重投影誤差關於相機位姿李代數的一階變化關係。保留了前面的負號，是因爲誤差由觀測值減預測值定義。可以反過來，定義成“預測值減觀測值”的形式，此時，去掉負號即可。如果 se(3) 的定義形式是旋轉在前，平移在後，只要把這個矩陣的前3列與後3列對調即可。
除了優化位姿，還需要優化特徵點的空間位置，需要討論 e 關於空間點 P 的導數。仍用鏈式法則：
$\frac{\partial e}{\partial P}=\frac{\partial e}{P'}\frac{\partial P'}{\partial P}$
第一項前面已經推導：
$\frac{\partial e}{\partial P'}=-\left[ \begin{matrix}\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{matrix}\right]=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]$
關於第二項，按照定義：
$P'=exp(\xi^{\wedge})P=RP+t$
P’ 對 P 求導後僅剩下 R ，於是：
$\frac{\partial e}{\partial P}=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]R$
因此，觀測相機方程關於相機位姿和特徵點的兩個導數矩陣都求到了，在優化過程中提供了重要的梯度方向，指導優化的迭代。
使用BA優化：