BA是一種優化方法，最小化重投影誤差優化PnP得到的R、t
應用場景：利用共視點的3D座標與相機內參矩陣，根據PnP計算位姿R、t（有誤差）。利用R、t及相機的投影模型可以計算出這些特徵點在第二幀圖像上的投影（有誤差），本身這些點在第二幀圖像上有真實投影，利用這兩個投影構建函數進行優化可以優化位姿。
1 BA解決的代價函數
觀測模型如下：
（1）世界座標系轉相機座標系
設P點的世界座標
（2）歸一化
![P^{'}_c = [u_c,v_c,1]^T=[x^{'}/z^{'},y^{'}/z^{'},1]^T](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?P%5E%7B%27%7D_c%20%3D%20%5Bu_c%2Cv_c%2C1%5D%5ET%3D%5Bx%5E%7B%27%7D/z%5E%7B%27%7D%2Cy%5E%7B%27%7D/z%5E%7B%27%7D%2C1%5D%5ET)
（3）去畸變
（4）相機座標系轉像素座標系
（5）得到相機的觀測模型：
（6）構建重投影誤差模型：
z是實際值，h是通過觀測模型得到的值，知識針對空間中一點。
2 LM算法求解
BA優化一般通過LM算法求解，參考最優化八：高斯牛頓法、LM法。
1中（6）方程中表示了一個點一個位姿構成的重投影誤差，利用多個位姿多個點構建優化方程（m個位姿，n個點）：
f(x)中的x是R、t、xyz。位姿用李代數表示，並將變量分爲兩部分，位姿
![[\xi,p ]= [\xi_1,\xi_2,\xi_3...,\xi_m, p_1,p_2,p_3,...,p_n]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5B%5Cxi%2Cp%20%5D%3D%20%5B%5Cxi_1%2C%5Cxi_2%2C%5Cxi_3...%2C%5Cxi_m%2C%20p_1%2Cp_2%2Cp_3%2C...%2Cp_n%5D)
則優化目標變爲問題如下：
優化函數f(x)有m+n個變量待求取，一般用LM法求解，爲什麼不用最速下降法？我覺得是最速下降法迭代次數過多，效率低。LM法要用到梯度，梯度計算如下：
![[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5B%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_n%7D%5D)
參考最優化八：高斯牛頓法、LM法，針對函數f(x)求解更新變量
J是一階導數。爲了計算方便，將一階梯度替換如下：
![[F,E]=[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5BF%2CE%5D%3D%5B%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_n%7D%5D)
F代表
![[F E]^T\cdot [F E] \bigtriangleup x=g](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5BF%20E%5D%5ET%5Ccdot%20%5BF%20E%5D%20%5Cbigtriangleup%20x%3Dg)
等式左邊展開並用H表示得到：
![H=\left [ \begin{matrix} F^TF & F^TE \\ E^TF & E^TE \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?H%3D%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20F%5ETF%20%26%20F%5ETE%20%5C%5C%20E%5ETF%20%26%20E%5ETE%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
目的是求解（2-7）中的
3 H矩陣的稀疏性
這據說是21世紀最重要的發現。
最優化問題
![[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]= [\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial \xi _1},...,\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial \xi _m},\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial p _1},...,\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial p _n}]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5B%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_n%7D%5D%3D%20%5B%5Csum_%7Bm%7D%5E%7Bi%3D1%7D%5Csum_%7Bn%7D%5E%7Bj%3D1%7D%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Csum_%7Bm%7D%5E%7Bi%3D1%7D%5Csum_%7Bn%7D%5E%7Bj%3D1%7D%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Csum_%7Bm%7D%5E%7Bi%3D1%7D%5Csum_%7Bn%7D%5E%7Bj%3D1%7D%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Csum_%7Bm%7D%5E%7Bi%3D1%7D%5Csum_%7Bn%7D%5E%7Bj%3D1%7D%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20p%20_n%7D%5D)
即：
![[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]= \sum_{i,j}[\frac{\partial e_{ij}}{\partial \xi _1},...,\frac{\partial e_{ij}}{\partial \xi _m},\frac{\partial e_{ij}}{\partial p _1},...,\frac{\partial e_{ij}}{\partial p _n}]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5B%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20p%20_n%7D%5D%3D%20%5Csum_%7Bi%2Cj%7D%5B%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20%5Cxi%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20%5Cxi%20_m%7D%2C%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20p%20_1%7D%2C...%2C%5Cfrac%7B%5Cpartial%20e_%7Bij%7D%7D%7B%5Cpartial%20p%20_n%7D%5D)
對其中一項令：
則：
![H = \sum_{i,j}J_{ij}^TJ_{ij}=\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?H%20%3D%20%5Csum_%7Bi%2Cj%7DJ_%7Bij%7D%5ETJ_%7Bij%7D%3D%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D%20%26%20H_%7B12%7D%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
H的稀疏性如下：
（1）
（2）
利用H的稀疏性計算方程（2-7）需要用到蘇耳消元法。
4 Schur消元
求解問題如下：
![\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D%20%26%20H_%7B12%7D%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
![\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]\bigtriangleup x=g](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D%20%26%20H_%7B12%7D%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%5Cbigtriangleup%20x%3Dg)
將
![\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]= \left [ \begin{matrix} v \\ w \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D%20%26%20H_%7B12%7D%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20%5Cbigtriangleup%20%5Cxi%20%5C%5C%20%5Cbigtriangleup%20p%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%3D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20v%20%5C%5C%20w%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
消去矩陣H中右上角部分，等式左右同乘以矩陣：
![\left [ \begin{matrix} I & -H_{21}^{T}H_{22}^{-1} \\ 0 & I \\ \end{matrix} \right ] \left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]=\left [ \begin{matrix} I & -H_{21}^{T}H_{22}^{-1} \\ 0 & I \\ \end{matrix} \right ] \left [ \begin{matrix} v \\ w \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20I%20%26%20-H_%7B21%7D%5E%7BT%7DH_%7B22%7D%5E%7B-1%7D%20%5C%5C%200%20%26%20I%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D%20%26%20H_%7B12%7D%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20%5Cbigtriangleup%20%5Cxi%20%5C%5C%20%5Cbigtriangleup%20p%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%3D%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20I%20%26%20-H_%7B21%7D%5E%7BT%7DH_%7B22%7D%5E%7B-1%7D%20%5C%5C%200%20%26%20I%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20v%20%5C%5C%20w%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
整理得到：
![\left [ \begin{matrix} H_{11}- H_{21}^TH_{22}^{-1}H_{21}& 0 \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]= \left [ \begin{matrix} v-H_{21}^TH_{22}^{-1}w \\ w \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20H_%7B11%7D-%20H_%7B21%7D%5ETH_%7B22%7D%5E%7B-1%7DH_%7B21%7D%26%200%20%5C%5C%20H_%7B21%7D%20%26%20H_%7B22%7D%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20%5Cbigtriangleup%20%5Cxi%20%5C%5C%20%5Cbigtriangleup%20p%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%3D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20v-H_%7B21%7D%5ETH_%7B22%7D%5E%7B-1%7Dw%20%5C%5C%20w%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
方程第一項與爲位置點p無關，取出：
![\left [ H_{11}- H_{21}^TH_{22}^{-1}H_{21} \right ] \cdot \left [ \begin{matrix}\bigtriangleup \xi \\ \end{matrix} \right ]= \left [ \begin{matrix} v-H_{21}^TH_{22}^{-1}w \\ \end{matrix} \right ]](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20H_%7B11%7D-%20H_%7B21%7D%5ETH_%7B22%7D%5E%7B-1%7DH_%7B21%7D%20%5Cright%20%5D%20%5Ccdot%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%5Cbigtriangleup%20%5Cxi%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D%3D%20%5Cleft%20%5B%20%5Cbegin%7Bmatrix%7D%20v-H_%7B21%7D%5ETH_%7B22%7D%5E%7B-1%7Dw%20%5C%5C%20%5Cend%7Bmatrix%7D%20%5Cright%20%5D)
求解
消元優勢在於矩陣