后端之非线性优化

后端之非线性优化

前言

在后端优化中，存在着两种优化算法，一种是利用矩阵分解求得最小二乘问题解的方法，不用迭代；第二种就是利用迭代的方式，不断调整参数，使残差最小，这种算法是本章要介绍的算法，也是我们通常所说的非线性优化的方法。

非线性优化

这里有类似于机器学习中监督学习的概念，使用预测值与观测值构建出一个残差，而状态值与环境观测则是需要调节的变量。把所有残差求和，迭代更新变量值，使loss不断下降，到一个最小值时，这时的状态量就被称之为最优估计值。

这里的loss，一般会构建为观测与预测的差值的平方，所以构建的问题一般为最小二乘问题。

牛顿法

牛顿法来源于求方程解，但求极值的过程，也是求函数导数为0的过程，所以二者本质上是一样。记残差函数为：

F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 F(x) = \frac {1}{2}||f(x)||^2 F(x)=21∣∣f(x)∣∣2

对F(x)进行二阶泰勒展开

F ( x ) = F ( x 0 ) + F ′ ( x 0 ) ( x − x 0 ) + 1 2 F ′ ′ ( x 0 ) ( x − x 0 ) 2 + σ 2 ( x − x 0 ) 2 F(x) = F(x_0) + F^{'}(x_0)(x-x_0)+\frac{1}{2}F^{''}(x_0)(x-x_0)^2 +\sigma^2(x-x_0)^2 F(x)=F(x0)+F′(x0)(x−x0)+21F′′(x0)(x−x0)2+σ2(x−x0)2

对F(x)的泰勒展开求导：

F ′ ( x ) = F ′ ( x 0 ) + F ′ ′ ( x 0 ) ( x − x 0 ) F^{'}(x) = F^{'}(x_0)+F^{''}(x_0)(x-x_0) F′(x)=F′(x0)+F′′(x0)(x−x0)

令导数为0，得到

F ′ ′ ( x 0 ) ( x − x 0 ) = − F ′ ( x 0 ) F^{''}(x_0)(x-x_0) = -F^{'}(x_0) F′′(x0)(x−x0)=−F′(x0)

将 x − x 0 x-x_0 x−x0视作 x 0 x_0 x0基础上的增量 Δ x \Delta x Δx，则 x = x 0 + Δ x x = x_0 +\Delta x x=x0+Δx，于是原方程变为

F ′ ′ ( x 0 ) Δ x = − F ′ ( x 0 ) F^{''}(x_0)\Delta x = -F^{'}(x_0) F′′(x0)Δx=−F′(x0)

对于变量为向量的情况，这里的一阶导数与二阶导数，就变为雅克比矩阵J与海森矩阵H。上式可变为：

H ( x 0 ) Δ x = − J ( x 0 ) H(x_0)\Delta x = -J(x_0) H(x0)Δx=−J(x0)

求得增量 Δ x \Delta x Δx后，更新 x 0 x_0 x0，在计算新x下loss，重复求增量，不断迭代直到收敛，最后得到的x就是我们要求的最优变量。

上面的方程也称为增量方程。

高斯牛顿法

对于复杂函数，特别是非线性函数，求二阶导数十分困难，甚至是难以求出的，牛顿法中直接求海森矩阵的做法十分困难，因此出现了高斯牛顿法：

具体做法是对f(x)做一阶泰勒展开，原loss函数变为

F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 = 1 2 ∣ ∣ f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) ∣ ∣ 2 = 1 2 ( ∥ f ( x 0 ) ∥ 2 + 2 f ( x 0 ) f ′ ( x 0 ) ( x − x 0 ) + f ′ ( x 0 ) 2 ( x − x 0 ) 2 ) \begin{array}{l} F(x) = \frac {1}{2}||f(x)||^2 = \frac {1}{2}||f(x_0)+f^{'}(x_0)(x-x_0)||^2 \\ =\frac{1}{2}\left(\|f(x_0)\|^{2}+2 f(x_0) f^{'}(x_0) (x-x_0)+ f^{'}(x_0)^2(x-x_0)^2 \right) \end{array} F(x)=21∣∣f(x)∣∣2=21∣∣f(x0)+f′(x0)(x−x0)∣∣2=21(∥f(x0)∥2+2f(x0)f′(x0)(x−x0)+f′(x0)2(x−x0)2)

对上式关于x求导，得：

F ′ ( x ) = f ( x 0 ) f ′ ( x 0 ) + f ′ ( x 0 ) 2 ( x − x 0 ) \begin{array}{l} F^{'}(x)= f(x_0) f^{'}(x_0) + f^{'}(x_0)^2(x-x_0) \end{array} F′(x)=f(x0)f′(x0)+f′(x0)2(x−x0)

令导数等于0，有：

f ′ ( x 0 ) 2 ( x − x 0 ) = − f ( x 0 ) f ′ ( x 0 ) f^{'}(x_0)^2(x-x_0) = -f(x_0) f^{'}(x_0) f′(x0)2(x−x0)=−f(x0)f′(x0)

同样的，令 x − x 0 x-x_0 x−x0为 Δ x \Delta x Δx，对于多元函数，一阶导为雅克比矩阵，于是原方程变为：

J ( x 0 ) J ( x 0 ) T Δ x = − f ( x 0 ) J ( x 0 ) J(x_0)J(x_0)^{T} \Delta x = -f(x_0)J(x_0) J(x0)J(x0)TΔx=−f(x0)J(x0)

同样的方法，求出增量x后，更新x，得到当前loss，不断迭代下去直到收敛。

对比于牛顿法的增量方程，可以看到高斯牛顿法左边用更容易求导的雅克比矩阵的乘积，近似代替了海森矩阵。右侧则多乘了一个 f ( x 0 ) f(x_0) f(x0)

H ( x 0 ) Δ x = − J ( x 0 ) H(x_0)\Delta x = -J(x_0) H(x0)Δx=−J(x0)

列文伯格-马夸而特法

在求增量方程的解时，涉及矩阵求逆，如下：

Δ x = − H ( x 0 ) − 1 J ( x 0 ) \Delta x = -H(x_0)^{-1}J(x_0) Δx=−H(x0)−1J(x0)

在高斯牛顿法中, H ( x 0 ) H(x_0) H(x0)对应 J ( x 0 ) J ( x 0 ) T J(x_0)J(x_0)^T J(x0)J(x0)T

但是，对于高斯牛顿法，并不能保证 H ( x 0 ) H(x_0) H(x0)可逆，另外，如果增量 Δ x \Delta x Δx过大，线性近似的误差也会很大，为了解决这两种问题，出现了列文伯格-马夸而特法，也就是LM法。

LM法是对高斯牛顿法（GN法）的改进，通过引入信赖区域的残差，对近似范围进行了限制，其中的残差为：

min ⁡ 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 , s.t ∥ D Δ x k ∥ 2 ≤ μ \min \frac{1}{2}\left\|f\left(x_k\right)+J\left(x_k\right)^T \Delta x_k\right\|^2, \quad \text { s.t } \quad\left\|D \Delta x_k\right\|^2 \leq \mu min21 f(xk)+J(xk)TΔxk 2, s.t ∥DΔxk∥2≤μ

增量方程的获取，构建拉格朗日函数， λ \lambda λ是系数因子：

L ( Δ x , λ ) = 1 2 ∥ f ( x ) + J ( x ) T Δ x ∥ 2 + λ 2 ( ∥ D Δ x ∥ 2 − μ ) L(\Delta x, \lambda)=\frac{1}{2}\left\|f(x)+J(x)^T \Delta x\right\|^2+\frac{\lambda}{2}\left(\|D \Delta x\|^2-\mu\right) L(Δx,λ)=21 f(x)+J(x)TΔx 2+2λ(∥DΔx∥2−μ)

这样的话，化简后求导就可以得到：

J ( x ) f ( x ) + J ( x ) J T ( x ) Δ x + λ D T D Δ x = 0 J(x) f(x)+J(x) J^T(x) \Delta x+\lambda D^T D \Delta x=0 J(x)f(x)+J(x)JT(x)Δx+λDTDΔx=0

我们化简后得到：

( J J T + λ D T D ) Δ x = − J f \left(J J^T+\lambda D^T D\right) \Delta x=-J f (JJT+λDTD)Δx=−Jf

在本文中，我们令 H = J J T , g = − J f H=J J^T, g=-J f H=JJT,g=−Jf。在实际使用中，通常用 I I I来代替 D T D D^TD DTD。所以，公式就变为：

( H + λ I ) Δ x k = g (H+\lambda I) \Delta x_k=g (H+λI)Δxk=g

这样就得到了增量方程。

LM称为置信区域法，其会对线性近似程度的好坏进行度量，通过如下系数计算：

ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x \rho=\frac{f(x+\Delta x)-f(x)}{J(x)^{T} \Delta x} ρ=J(x)TΔxf(x+Δx)−f(x)

该分数代表近似的准确性，在一定范围内，会认为近似是有效的，因此LM法也称为基于区域搜索的方法，及信赖区域法，可以看到其分为以下几种情况：

○ ρ 接近1，近似是好的，不需要更改；

○ ρ 太小，则实际减少的值小于近似减少的值，近似较大，需要缩小近似的范围；

○ ρ 太大，则实际减少的值大于近似减少的值，近似较小，需要扩大近似的范围。

根据ρ的值，来进行步长的动态调整，即改变增量方程中的λ值。- ρ 越大，表明 L对F的近似效果越好，所以可以增大 λ以使得LM法接近梯度下降法

相反，ρ 越小，表明 L的近似越差，所以应该减小μ以使得迭代公式接近高斯牛顿法。

由于系数矩阵中加入了λ乘以单位矩阵，可以保证系数矩阵的正定可逆。其次，通过改变λ值，可以起到调整步长的作用，当线性近似程度不好时，减小更新的步长，可以有更好的收敛性。

BA

BA是VSLAM里广泛使用的技术，它通过同时优化地图点与相机位姿，使空间3维点与相机平面观测的误差最小，每个3D点光束都能穿过观测相机的光心。BA是VSLAM里常用的后端算法，但由于其灵活性，也可以在前端使用。相比于上面几种线性代数的方法，BA一方面是非线性方法，可以最大程度使用所有匹配点数据，另一方面可以优化三维点坐标，被称为slam中万金油的方法。

BA不仅可以求两帧之间的位移，还可以求多帧的姿态。如果固定住路标点，只优化位姿，这种BA叫做运动BA。而把路标点与姿态一起优化叫做完整BA。由于优化变量数量庞大，完整BA优化比较耗时。

BA优化时需要给一个迭代的初始值，一般做法是不直接进行BA，而是先用前面介绍的PnP算法或者对极几何先获得相机位姿及3D点坐标初始值，然后把初始值交给BA做最终的优化调整。

第一步，构建残差

BA残差为特征点观测值与预测值之差的平方，其中观测值为像素坐标系下特征点坐标，预测值为3D点在该相机坐标系下的像素平面投影坐标。这里需要把3D点转换到当前相机坐标系，假设3D点为点P，相机相对旋转与平移为R,t

P ′ = R p + t P^{\prime} = Rp+t P′=Rp+t

P'在归一化平面上坐标为：

P c = [ u c , v c , 1 ] = [ P x ′ / P z ′ , P y ′ / P z ′ , 1 ] P_c = [u_c,v_c,1] = [P^{\prime}_x/P^{\prime}_z,P^{\prime}_y/P^{\prime}_z,1] Pc=[uc,vc,1]=[Px′/Pz′,Py′/Pz′,1]

通过相机镜头时，存在畸变，则畸变坐标为：

{ u c ′ = u c ( 1 + k 1 r c 2 + k 2 r c 4 ) v c ′ = v c ( 1 + k 1 r c 2 + k 2 r c 4 ) \left\{\begin{array}{l} u_{\mathrm{c}}^{\prime}=u_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \\ v_{\mathrm{c}}^{\prime}=v_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \end{array}\right. {uc′=uc(1+k1rc2+k2rc4)vc′=vc(1+k1rc2+k2rc4)

根据相机模型，最后像素坐标为：

{ u s = f x ∗ u c ′ + c x v s = f y ∗ v c ′ + c y \left\{\begin{array}{l} u_{\mathrm{s}}= f_x*u_{\mathrm{c}}^{\prime} + c_x \\ v_{\mathrm{s}}= f_y*v_{\mathrm{c}}^{\prime} + c_y \end{array}\right. {us=fx∗uc′+cxvs=fy∗vc′+cy

以上，从原始3D点，到观测点的转换，称为观测模型，这里记为：

z = h ( T , p ) z = h(T,p) z=h(T,p)

其中T为包含旋转与平移的转换矩阵。

转换过程如上图：

现在，设相机平面观测像素坐标为z',则观测误差为：

e = z ′ − h ( T , p ) e = z^{\prime} - h(T,p) e=z′−h(T,p)

将所有点的观测残差平方求和，得到如下观测残差：

1 2 ∑ i = 1 m ∑ j = 1 n ∥ e i j ∥ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∥ z i j − h ( T i , p j ) ∥ 2 . \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}\left\|\bm{e}{ij} \right\|^2= \frac{1}{2}\sum{i=1}^{m}\sum_{j=1}^{n}\left\| \bm{z}_{ij}-h(T_i,p_j)\right\|^2. 21∑i=1m∑j=1n∥eij∥2=21∑i=1m∑j=1n∥zij−h(Ti,pj)∥2.

该残差为最小二乘残差，对其求最小化的过程，同时对位姿与3D点进行了调整，就是所谓的BA.

可以看到，这个残差也是通过3D-2D点的关联完成的。

第二步，构建增量方程

将所有待求变量，写为一个向量形式为：

x = [ T 1 , ⋯   , T m , p 1 , ⋯   , p n ] T . \bm{x}=[T_1,\cdots,T_m,p1,\cdots,p_n]^\text{T}. x=[T1,⋯,Tm,p1,⋯,pn]T.

向量中  ⁣ T  ⁣ ⁣ \!T\!\! T表示相机位姿，即旋转与平移，  ⁣ p  ⁣ \!p\! p表示3D点坐标，如果是求两帧之间相对位移，则其中  ⁣ T  ⁣ \!T\! T就只有  ⁣ T 1  ⁣ \!T_1\! T1。假设残差函数为  ⁣ f ( x )  ⁣ \!f(x)\! f(x)，则当我们在 x x x上加一个增量 Δ x \Delta_x Δx时，  ⁣ f ( x )  ⁣ \!f(x)\! f(x)的变化为：

1 2 ∥ f ( x + Δ x ) ∥ 2 ≈ 1 2 ∑ i = 1 m ∑ i = 1 n ∥ e i j + F i j Δ ξ i + E i j Δ p j ∥ 2 \frac{1}{2}\| f(x+\Delta x) \|^2 \approx \frac{1}{2}\sum_{i=1}^{m}\sum_{i=1}^{n} \| e_{ij}+F_{ij}\Delta\xi_i+E_{ij}\Delta p_j \|^2 21∥f(x+Δx)∥2≈21∑i=1m∑i=1n∥eij+FijΔξi+EijΔpj∥2

其中， F F F表示整个代价函数在当前状态下对相机姿态的偏导数，而 E E E表示该函数对路标点位置的偏导。以上是对每一个点进行求和，现在把每个点及每个姿态，各自放在一起，形成一个向量，则上述式子变为：

1 2 ∥ f ( x + Δ x ) ∥ 2 = 1 2 ∥ e + F Δ x c + E Δ x p ∥ 2 \frac{1}{2}\| f(x+\Delta x) \|^2 = \frac{1}{2} \| e+F\Delta x_c+E\Delta x_p \|^2 21∥f(x+Δx)∥2=21∥e+FΔxc+EΔxp∥2

对该函数求最小值，需要用到非线性优化中的最小二乘法，不对其进行展开讨论，只说明一般形式，不管选择何种最小二乘中的高斯牛顿还是LM法，最后都需要求增量方程，该方程由上式求导数为0得来：

H Δ x = g . \bm{H}\Delta \bm{x}=\bm{g}. HΔx=g.

如果是高斯牛顿法，则增量方程如下：

J T ( x 0 ) J ( x 0 ) Δ x = − J T ( x 0 ) f ( x 0 ) J^T(x_0)J(x_0)\Delta x=-J^T(x_0)f(x_0) JT(x0)J(x0)Δx=−JT(x0)f(x0)

高斯牛顿法和列文伯格一马夸尔特方法的主要差别在于，这里的 H H H是取 J T J J^TJ JTJ还是 J T J + λ I J^TJ+\lambda I JTJ+λI的形式。由于把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为

J = [ F    E ] J=[F\,\, E] J=[FE]

那么以高斯牛顿法为例， H H H矩阵为如下形式：

H = J T J = [ F T F F T E E T F E T E ] H=J^TJ= \begin{bmatrix} F^TF & F^TE \\ E^TF & E^TE \end{bmatrix} H=JTJ=[FTFETFFTEETE]

因为该方法包含了所有变量，一般特征点有几百个，这个 H H H矩阵会非常大，在求增量时，如果直接求矩阵的逆，其运算复杂度为 O ( n 3 ) O(n^3) O(n3)，计算会非常耗时，但幸运的是， H H H矩阵本身具有稀疏性，通过这个性质，可以加速增量方程的求解。

第三步，通过边缘化求解增量方程：

H H H矩阵的稀疏性是由雅可比矩阵 J ( x ) J(x) J(x)引起的。考虑这些代价函数当中的其中一个 e  ⁣ e\! e。注意，这个误差项只描述了在 T T T看到 P P P这件事，只涉及第  ⁣ i  ⁣ \!i\! i个相机位姿和第  ⁣ j  ⁣ \!j\! j个路标点，对其余部分的变量的导数都为0。所以该误差项对应的雅可比矩阵有下面的形式：

J i j ( x ) = ( 0 2 × 6 , . . . , 0 2 × 6 , ∂ e i j ∂ T i , 0 2 × 6 , . . . , 0 2 × 3 , . . . , 0 2 × 3 , ∂ e i j ∂ p j , 0 2 × 3 , . . . , 0 2 × 3 ) . J_{ij}(x)=\left( \bm{0}{2\times6},...,\bm{0}{2\times6},\frac{\partial \bm{e}{ij}}{\partial \bm{T}i}, \bm{0}{2\times6},...,\bm{0}{2\times3},...,\bm{0}{2\times3},\frac{\partial \bm{e}{ij}}{\partial \bm{p}j}, \bm{0}{2\times3},...,\bm{0}_{2\times3} \right). Jij(x)=(02×6,...,02×6,∂Ti∂eij,02×6,...,02×3,...,02×3,∂pj∂eij,02×3,...,02×3).

其中  ⁣ 0 2 × 6  ⁣ \!\bm{0}{2\times 6}\! 02×6表示维度为  ⁣ 2  ⁣ ×  ⁣ 6  ⁣ \!2\!\times\!6\! 2×6的  ⁣ 0  ⁣ \!\bm{0}\! 0矩阵，同理，  ⁣ 0 2 × 3  ⁣ \!\bm{0}{2\times 3}\! 02×3也是一样的。该误差项对相机姿态的偏导维度为  ⁣ 2  ⁣ ×  ⁣ 6  ⁣ \!2\!\times\!6\! 2×6，对路标点的偏导维度是  ⁣ 2  ⁣ ×  ⁣ 3  ⁣ \!2\!\times\!3\! 2×3。这个误差项的雅可比矩阵，除了这两处为非零块，其余地方都为零。这体现了该误差项与其他路标和轨迹无关的特性。

以下图为例，我们设  ⁣ J i j  ⁣ \!J_{ij}\! Jij只在  ⁣ i  ⁣ ,  ⁣ j  ⁣ \!i\!,\!j\! i,j处有非零块，那么它对  ⁣ H  ⁣ \!H\! H的贡献为  ⁣ ⁣ J i j T J i j  ⁣ \!\!J_{ij}^TJ_{ij}\! JijTJij，具有图上所画的稀疏形式。这个  ⁣ J i j T J i j  ⁣ \!J_{ij}^TJ_{ij}\! JijTJij矩阵也仅有4个非零块，位于  ⁣ ( i , i ) ,  ⁣ ( i , j ) ,  ⁣ ( j , i ) ,  ⁣ ( j , j )  ⁣ \!(i,i),\!(i,j),\!(j,i),\!(j,j)\! (i,i),(i,j),(j,i),(j,j)。对于整体的  ⁣ H  ⁣ \!H\! H，有

H = ∑ i , j J i j T J i j , \bm{H}=\sum_{i,j}\bm{J}{ij}^{T}\bm{J}{ij}, H=∑i,jJijTJij,

请注意，  ⁣ i  ⁣ \!i\! i在所有相机位姿中取值，  ⁣ j  ⁣ \!j\! j在所有路标点中取值。我们把  ⁣ H  ⁣ \!H\! H进行分块：

H = [ H 11 H 12 H 21 H 22 ] . H= \begin{bmatrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{bmatrix}. H=[H11H21H12H22].

则J的稀疏性对最后  ⁣ H \!H H矩阵稀疏性的影响如下：

则对原 H H H矩阵，可以做如下划分：

于是，对应的线性方程组也可以由  ⁣ H Δ x = g \!H \Delta x = g HΔx=g变为如下形式：

[ B E E T C ] [ Δ x c Δ x p ] = [ v w ] . \begin{bmatrix} \bm{B} & \bm{E} \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta \bm{x}_c \\ \Delta \bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{v} \\ \bm{w} \end{bmatrix}. [BETEC][ΔxcΔxp]=[vw].

其中 B \bm{B} B是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以  ⁣ C  ⁣ \!\bm{C}\! C 往往也远大于  ⁣ B  ⁣ \!\bm{B}\! B。三维空间中每个路标点为三维，于是  ⁣ C  ⁣ \!\bm{C}\! C 矩阵为对角块矩阵，每个块为 3  ⁣ ×  ⁣ 3  ⁣ 3\!\times \!3\! 3×3 矩阵。对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求逆即可。考虑到这个特性，我们对线性方程组进行高斯消元，目标是消去右上角的非对角部分E,得

[ I − E C − 1 0 I ] [ B E E T C ] [ Δ x c Δ x p ] = [ I − E C − 1 0 I ] [ v w ] . \begin{bmatrix} \bm{I} & -\bm{EC}^{-1} \\ 0 & \bm{I} \end{bmatrix} \begin{bmatrix} \bm{B} & \bm{E} \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta\bm{x}_c \\ \Delta\bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{I} & -\bm{EC}^{-1} \\ 0 & \bm{I} \end{bmatrix} \begin{bmatrix} \bm{v} \\ \bm{w} \end{bmatrix}. [I0−EC−1I][BETEC][ΔxcΔxp]=[I0−EC−1I][vw].

整理，得

[ B − E C − 1 E T 0 E T C ] [ Δ x c Δ x p ] = [ v − E C − 1 w w ] . \begin{bmatrix} \bm{B}-\bm{EC}^{-1}\bm{E}^T & 0 \\ \bm{E}^T & \bm{C} \end{bmatrix} \begin{bmatrix} \Delta\bm{x}_c \\ \Delta\bm{x}_p \end{bmatrix} = \begin{bmatrix} \bm{v}-\bm{EC}^{-1}\bm{w} \\ \bm{w} \end{bmatrix}. [B−EC−1ETET0C][ΔxcΔxp]=[v−EC−1ww].

消元之后，方程组第一行变成和 Δ x \Delta\bm{x} Δx无关的项。单独把它拿出来，得到关于位姿部分的增量方程：

[ B − E C − 1 E T ] Δ x c = v − E C − 1 w . [\bm{B}-\bm{EC}^{-1}\bm{E}^T]\Delta \bm{x}_c=\bm{v}-\bm{EC}^{-1}\bm{w}. [B−EC−1ET]Δxc=v−EC−1w.

这个线性方程的维度和  ⁣ B  ⁣ \!B\! B 矩阵一样。我们的做法是先求解这个方程，然后把解得的  ⁣ Δ x  ⁣ \!\Delta\bm{x}\! Δx 代入原方程，求解  ⁣ Δ x p  ⁣ \!\Delta\bm{x}_p\! Δxp。这个过程称为Marginalization，或者Schur消元（Schur Elimination）。相比于直接解线性方程的做法，它的优势在于：

1.在消元过程中，由于  ⁣ C \!\bm{C} C为对角块，所以 C − 1  ⁣ \bm{C}^{-1}\! C−1容易解出。

2.求解了  ⁣ ⁣ Δ x c  ⁣ \!\!\Delta\bm{x}_c\! Δxc之后，路标部分的增量方程由  ⁣ Δ x p  ⁣ =  ⁣ C − 1 ( w − E T Δ x c )  ⁣ \!\Delta\bm{x}_p\!=\!\bm{C}^{-1}(\bm{w}-\bm{E}^T\Delta\bm{x}_c)\! Δxp=C−1(w−ETΔxc)给出。这依然用到了 C − 1  ⁣ \bm{C}^{-1}\! C−1易于求解的特性。

最后，在求得 x \bm{x} x的增量后，更新变量值，不断迭代直到问题收敛，最后求得最优的相机位姿及3D点坐标。需要注意的是，BA优化中需要给一个迭代的初始值，一般做法是不直接进行BA，而是先用PnP算法或者对极几何先获得相机位姿及3D点坐标初始值，然后把初始值交给BA做最终的优化调整。