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)
求解
消元优势在于矩阵