BA

Bundle Adjustment問題在後端用來優化相機位姿${\xi}$(李代數形式)和觀測到的路標點p(3D點)，實質上是一個最小二乘問題，下面進行推導

BA求解過程

在視覺里程計中可以利用pnp、icp、對極幾何等方法求得相機的位姿${\xi}$，也就是旋轉矩陣R和平移向量t。
路標點與畫素點的關係是:
${z=\dbinom{u}{v}=\frac{1}{s}K(Rp+t)_{1:2}}$
簡寫成
$z=h(x,y)$
$x$表示某一時刻相機位姿，對應的李代數為$\xi$，$y$

y為觀測的路標點，即三維點p，由於存在噪聲等的影響，求得的畫素點與觀測到的實際的畫素點存在誤差：
$e=z-h(\xi,p)$
假設一共求得了m個時刻的相機位姿以及觀測到n個路標點，把所有時刻觀測到的路標點的誤差都考慮進來，令$z_{i,j}$表示在位姿$\xi_{i}$下觀測路標$p_{j}$得到的畫素點座標，則構建整體的代價函式為：
$\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}||^{2}=\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||z_{ij}-h(\xi_{i},p_{j})||^{2}$

根據在視覺里程計中求得的相機位姿和路標點作為初值，不斷尋找下降方向求得最優的相機位姿和路標點使得整體代價函式最小，這就是BA優化問題
把所有的待優化引數放在一個向量裡：
$x=[\xi_{1},..,\xi_{m},p_{1},..,p_{n}]^{T}$
則有
$\frac{1}{2}||f(x)||^{2}=\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}||^{2}$

給自變數一個增量$\Delta x=(\Delta \xi_{1},..,\Delta \xi_{m},\Delta p_{1},..,\Delta p_{n})^{T}=(\Delta x_{c},\Delta x_{p})^{T}$，目標函式變為
$\frac{1}{2}||f(x+\Delta x)||^{2}=\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||z_{ij}-h(\xi_{i}+\Delta \xi_{i},p_{j}+\Delta p_{j})||^{2}$
對$h$一階泰勒展開
$h(\xi_{i}+\Delta \xi_{i},p_{j}+\Delta p_{j})||^{2}\approx h(\xi_{i},p_{j})+h_{\Delta \xi_{i}}^{'}\Delta \xi_{i}+h_{\Delta p_{j}^{'}\Delta p_{j}}$
則$\frac{1}{2}||f(x+\Delta x)||^{2}\approx \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+F_{ij}\Delta \xi_{i}+E_{ij}\Delta p_{j}||^{2}=\frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+J_{ij}\Delta x||^{2}$
[上述式子因為有平方所以負號可以去掉(z-h變成h-z)]
其中$J_{ij}=(0_{2*6},..,0_{2*6},F_{ij},0_{2*6},..0_{2*3},..0_{2*3},E_{ij},..,0_{2*3})$