《增强现实：原理、算法与应用》读书笔记（5）运动恢复结构（上）初始化、相机位姿估计、集束调整

运动恢复结构（SfM）是一种从运动的相机拍摄的图像或视频序列中自动地恢复出相机运动轨迹以及场景三维结构的技术。

如图，一个完整的SfM系统一般包括特征匹配、初始化、相机位姿和特征点三维位置的求解、集束调整（bundle adjustment，BA）和自标定（self-calibration）等模块。

早期的SfM系统一般是离线计算的，后来发展出了实时的SfM技术，也就是视觉SLAM技术。

初始化

早期的SfM系统一般采用基于矩阵分解的方法（Tomasi et al., 1992; Sturm et al., 1996），通过特征匹配的结果构造一个观测矩阵，并从观测矩阵中分解出所有相机的投影矩阵和所有三维点的齐次坐标。这类方法通常对观测结果有很高的要求，例如要求每个三维点在每个相机中都可见，且不存在误匹配，实际情况下很难满足。

目前大多SfM系统中采用增量式（Snavely et al., 2006; Zhang et al., 2007）的求解策略。这类求解方法一般需要提供比较好的初始化结果。常见的增量式SfM首先选择两帧或三帧图像来初始化场景，也就是恢复初始帧的相机运动参数以及对应的公共匹配点的三维位置。

如果相机内参已知，一般只需要两帧图像即可初始化。如果相机内参未知，那么可以先从两帧图像使用基础矩阵进行射影重建（projective reconstruction），即仅求解出投影矩阵，还未分解出旋转矩阵和平移向量，待后续更多图像加入优化后通过自定标技术升级为度量重建（metric reconstruction）。

用于初始化的图像需要至少满足以下两个条件：（1）有充足的公共点（特征匹配点足够多）；（2）足够长的基线，从而使得初始的公共点可以通过三角化计算出空间位置。

第一个条件很容易判断，第二个条件可以采用Polleyfeys等（2004）提出的基于图像的距离（image-based distance）来判断：
median{d(xi′,π(Hx^i))}median\left \{ d\left ( x_i',\pi \left ( H\hat{x}_i \right ) \right ) \right \}median{d(xi′,π(Hx^i))}

式中，xix_ixi和xi′x_i'xi′是两帧上对应的某对匹配点。d(xi′,π(Hx^i))d\left ( x_i',\pi \left ( H\hat{x}_i \right ) \right )d(xi′,π(Hx^i))是将原匹配点xix_ixi通过单应性变换（平面点对组变换）之后，与目标匹配点xi′x_i'xi′间的欧氏距离。在焦距已知或不变的情况下，根据上述两个条件一般能选出合适的图像对进行初始化。但在焦距未知且变化的情况下，还要考虑更多的因素。

当初始化图像对选取完毕后，可以根据得到的若干匹配点对，求解出两幅图像对应的投影矩阵，通常，会将其中的一个图像作为参考图像，即其相机坐标系作为世界坐标系，例如，在相机内参未知的情况下，可以先求出基础矩阵FFF，然后直接设定两张图像的投影矩阵为（Hartley et al., 2004）：
P=[I∣0]P=[I|0]P=[I∣0]P′=[[e′]×F∣e′]P'=[[e']_\times F|e']P′=[[e′]×F∣e′]

式中，PPP和P′P'P′是两帧对应的投影矩阵，e′e'e′是极点，满足关系Fe′=0Fe'=0Fe′=0。在相机内参已知的情况下，一般可以根据五点法直接求解出两帧的位姿RRR和ttt。

在求解出投影矩阵之后，可以进一步通过三角化的方法求解出匹配点的三维位置。如图3.8所示，假设某对匹配点对为(x,x′)\left ( x,x' \right )(x,x′)，在无噪声的情况下，两幅图像的相机光心(C,C′)\left ( C,C' \right )(C,C′)和匹配点(x,x′)\left ( x,x' \right )(x,x′)的连线可以相交于同一个三维点，即构成一个三角形。其对应的三维位置为XXX，那么应该满足以下等式：
x^∼PX^\hat {x}\sim P\hat{X}x^∼PX^x^′∼P′X^\hat {x}'\sim P'\hat{X}x^′∼P′X^

上述两式经过变换，可以得到如下线性方程组：
[up3⊤−p1⊤vp3⊤−p2⊤u′p′3⊤−p1⊤v′p′3⊤−p2⊤]X^=0\begin{bmatrix} up^{3\top} -p^{1\top} \\ vp^{3\top} -p^{2\top} \\ u'p'^{3\top} -p^{1\top} \\ v'p'^{3\top} -p^{2\top} \end{bmatrix}\hat{X}=0⎣⎢⎢⎡up3⊤−p1⊤vp3⊤−p2⊤u′p′3⊤−p1⊤v′p′3⊤−p2⊤⎦⎥⎥⎤X^=0

pi⊤p^{i\top}pi⊤代表PPP矩阵的第iii行。

由于噪声等原因，上述等式不会严格成立，即相机光心到成像平面中观测的匹配点的两条连线实际上并不相交，反映在成像平面上，观测点并不会严格落在对应的极线上，然后用非线性优化方法进一步优化重投影误差，即优化如下目标函数：
X∗=argminX∑i∥π(PiX^−xi)∥2X^*=\underset{X}{arg min}\sum_{i}^{}\left \| \pi\left ( P_i\hat{X}-x_i \right ) \right \|^2X∗=Xargmini∑∥∥∥π(PiX^−xi)∥∥∥2

式中，PiP_iPi为第iii张图像的投影矩阵，xix_ixi是三维点XXX在第iii张图像的二维观测点坐标。

相机位姿估计

基于已有的三维点云估计相机位姿是三维运动结构恢复技术中的重要步骤，在这一问题中，假设已经获得了已重建的三维点云和新图像中的二维特征点的匹配关系，且相机内参已知，然后利用PnP（perspective-n-point）方法通过这种3D-2D的对应关系求解相机的位姿。

常见的PnP方法有直接线性变换法（DLT）、P3P、EPnP、DLS、UPnP、AP3P等，除此之外，还有更通用的集束调整迭代求解方法。这些方法中都用应用到高等数值分析的相关知识。

直接线性变换法是通过构造一个线性方程组，直接线性地估计投影矩阵（虽然有12个参数，但因为尺度可以任意缩放，实际上只有11个自由度），需要6对匹配点进行求解（可以使用SVD分解加速求解），但是DLT方法忽略了投影矩阵的内在约束关系，由于需要估计11个参数，不仅效率较低，而且容易受到噪声的影响。但由于其实现比较简单，在对效率要求不高的情况下，可以用DLT方法先估计出投影矩阵PPP的初值，然后进一步分解出K,R,tK,R,tK,R,t，并用非线性优化方法进一步求解。

P3P方法仅需三对匹配点即可求解，该方法充分利用了匹配点对构成的空间三角形结构，基于余弦定理将空间几何关系转化为三个多元二次方程，将原先的3D-2D匹配问题转换为3D-3D的点云配准问题，可以方便使用SVDSVDSVD求解。

传统的这些PnP方法都属于线性的方法，在无噪声的情况下可以很好的解决问题，但实际情况中难免有噪声和误匹配，导致求解的结果不够精确。因为PnP问题可以被形式化为非线性最小二乘问题，所以也可以通过最小化重投影误差来求解投影矩阵：
Pi∗=argminPi∑j∥π(PiX^)−xij∥2P_i^*=\underset{P_i}{arg min}\sum_{j}^{}\left \| \pi\left ( P_i\hat{X}\right )-x_{ij} \right \|^2Pi∗=Piargminj∑∥∥∥π(PiX^)−xij∥∥∥2

式中，PiP_iPi为投影矩阵，xijx_{ij}xij是当前观测到的点云中三维点XjX_jXj对应的二维点坐标。

获得投影矩阵后，如果相机内参已经标定，可以根据P=K[R∣t]P=K[R|t]P=K[R∣t]，直接求出RRR和ttt；如果相机的内参未知，则可以使用RQRQRQ分解（相机内参矩阵KKK）是一个上矩阵。

通常情况下，一般会使用线性的方法（如EPnP）结合RANSAC鲁棒地估计出相机的位姿，然后通过优化目标函数来进一步求精，如果相机内参位置，则可以采用UPnP来估计相机的初始位姿。

集束调整

初始化之后，每加一张新的图像一般是先假设三维点不变求解这张图像的投影矩阵，然后固定投影矩阵三角化出新匹配上的三维点以及对已有的三维点进一步求精，但是这种策略是将相机参数和三维点的优化分离开，不是最优的，容易造成误差累积。

集束调整是将相机参数核三维点在一个目标函数中进行同时优化，理论上可以得到最优解。因此，在SfM求解过程中一般需要频繁地调用集束调整来优化相机参数和三维点，以减少或消除误差累积。

但是，对所有图像和三维点进行集束调整，计算复杂度很高，难以满足实时应用要求。随着实时的SfM算法需求日益增加，一些改进的集束调整算法应运而生，致力于在保证一定精度的前提下降低计算代价。一般主要通过两种策略来减少算法的计算量。一种是针对应用场景的特点减小集束调整问题的规模，具体来说有以下几种方式：

（1）减少变量个数：只保留相机状态量，而不保留三维点变量，如位姿图优化（pose graph optimization）算法；
（2）基于固定历史时间窗口（fixed-lag）：只估计最近一段固定时间内的历史状态；
（3）给予滑动窗口（sliding-window）：只估计最近的数个历史状态；
（4）基于关键帧：只估计一部分选定的携带了足够信息的历史状态等。

当然，以上办法也可以结合使用。

另一种策略是通过深入分析集束调整问题的特点，做针对性优化，减少冗余的计算。集束调整问题通常具有特殊的性质，合理利用这些性质，可以帮助我们更高效的求解。

比如，在SfM系统中，通常三维点变量的数量要远大于相机状态变量的数量，而且通常状态约束集中在状态与状态之间、状态与三维点之间，这就导致集束调整构建的标准方程具有特别的稀疏结构。

另外，在进行增量式集束调整的过程中，通常只有较新加入的变量会发生比较大的变动，只对少部分变量进行更新，从而减少集束调整的计算量。

标准的集束调整

SfM的核心问题在于优化如下目标：
argminKi,Ri,ti,Xj∑i=1m∑j=1n∥π(Ki(RiXj+ti))−xij∥2\underset{K_i,R_i,t_i,X_j}{arg min}\sum_{i=1}^{m}\sum_{j=1}^{n}\left \| \pi(K_i(R_iX_j+t_i))-x_{ij} \right \|^2Ki,Ri,ti,Xjargmini=1∑mj=1∑n∥π(Ki(RiXj+ti))−xij∥2

式中，mmm为相机个数，n为三维点个数n为三维点个数n为三维点个数，KiK_iKi、RiR_iRi和tit_iti分别为第iii个相机的内参矩阵、旋转矩阵和平移向量，XjX_jXj为第jjj个三维点的三维坐标，xijx_{ij}xij为该点在第iii张图像上的二维位置。

上述目标函数通过联合优化所有相机参数核三维点坐标，使得所有投影点与图像中匹配上的二维点尽可能重合。这一过程被称为集束调整。

集束调整本质上是个非线性最小二乘问题，与传统的非线性最小二乘问题区别在于，集束调整可以利用投影矩阵的稀疏特性极大提升求解效率，降低内存需求。

Gauss-Newton方法是一种经典的非线性优化算法，可以求解如下优化问题：

这里φ\varphiφ为待优化的所有变量，非线性函数e(φ)e(\varphi)e(φ)将φ\varphiφ映射为一个高维误差向量。非线性最小二乘的目标是通过最小化该误差向量的二范数得到一个最优解φ∗\varphi^*φ∗。对于非线性优化问题，需要使用迭代优化策略找到一个局部最优解。将当前迭代的结果标记为φ~\widetilde{\varphi}φ，并引入增量Δφ\Delta\varphiΔφ，使得

式中，对于线性变量（如平移向量ttt、三维点坐标XXX），⨁\bigoplus⨁就是普通加法，而对于非线性的旋转矩阵RRR，则为：

式中，指数函数exp(Δθ)exp(\Delta\theta)exp(Δθ)将一个三维向量Δθ\Delta\thetaΔθ映射到SO(3)SO(3)SO(3)。对于一个小增量Δθ\Delta\thetaΔθ，exp(Δθ)exp(\Delta\theta)exp(Δθ)可以近似为：

因此，原优化问题可以转化为：

至此，将待优化变量由φ\varphiφ转化为增量Δφ\Delta\varphiΔφ。为求解上述优化问题，将e(φ~⨁Δφ)e(\widetilde{\varphi}\bigoplus \Delta\varphi)e(φ⨁Δφ)在Δφ=0\Delta\varphi=0Δφ=0处线性展开，得到：

可以简写为：

式中，e~\widetilde ee和J~\widetilde JJ分别是φ~\widetilde \varphiφ处的误差向量及其Jacobian矩阵，则目标函数可近似为：

上式在∂E∂Δφ=0\frac{\partial E}{\partial \Delta\varphi}=0∂Δφ∂E=0处取得极值，即：

该式也称为正规方程，J~⊤J~\widetilde J^\top \widetilde JJ⊤J称为信息矩阵。高斯牛顿法每次迭代中构造并求解正规方程，并用求解结果Δφ\Delta \varphiΔφ更新φ~\widetilde \varphiφ：

随着迭代的不断进行，φ~\widetilde\varphiφ逐渐逼近最优解φ∗\varphi^*φ∗。

对于集束调整问题，将相机iii的运动参数KiK_iKi，RiR_iRi和tit_iti对应的增量标记为Δci\Delta c_iΔci，将三维点jjj坐标XjX_jXj对应的增量标记为Δxj\Delta x_jΔxj。将所有待优化变量Δci\Delta c_iΔci，Δxj\Delta x_jΔxj构成一个高维向量：
Δφ=(Δc⊤Δx⊤)⊤=(Δc1⊤⋯Δcm⊤Δx1⊤⋯Δxn⊤)\Delta \varphi =\begin{pmatrix} \Delta c^\top & \Delta x^\top \end{pmatrix}^\top=\begin{pmatrix} \Delta c_1^\top & \cdots & \Delta c_m^\top & \Delta x_1^\top & \cdots & \Delta x_n^\top \end{pmatrix}Δφ=(Δc⊤Δx⊤)⊤=(Δc1⊤⋯Δcm⊤Δx1⊤⋯Δxn⊤)

将三维点jjj在相机iii上的投影方程定义为：
eij=π(Ki(RiXj+ti))−xije_{ij}=\pi(K_i(R_iX_j+t_i))-x_{ij}eij=π(Ki(RiXj+ti))−xij

将eije_{ij}eij在当前线性化点处线性展开：
eij≈e~ij+J~ijcΔci+J~ijxΔxje_{ij}\approx \widetilde e_{ij}+\widetilde J_{ij}^c\Delta c_i+\widetilde J_{ij}^x\Delta x_jeij≈eij+JijcΔci+JijxΔxj

由上式可见，eije_{ij}eij对于相机iii、三维点jjj以外变量的雅可比矩阵均为0，所以，正规方程J~⊤J~Δφ=−J~⊤e~\widetilde J^\top\widetilde J\Delta\varphi=-\widetilde J^\top\widetilde eJ⊤JΔφ=−J⊤e中的雅可比矩阵J~\widetilde JJ将十分稀疏，且有着特殊的稀疏结构。利用这一性质，可快速构造正规方程。具体而言，集束调整的正规方程有着如下形式：
[UWW⊤V][ΔcΔx]=[uv]\begin{bmatrix} U & W\\ W^\top & V \end{bmatrix}\begin{bmatrix} \Delta c\\ \Delta x \end{bmatrix}=\begin{bmatrix} u\\ v \end{bmatrix}[UW⊤WV][ΔcΔx]=[uv]

式中，UUU和VVV分别是m×mm\times mm×m和n×nn\times nn×n的对角矩阵
U=diag(U1⋯Um)U=diag(U_1\cdots U_m)U=diag(U1⋯Um)V=diag(V1⋯Vn)V=diag(V_1\cdots V_n)V=diag(V1⋯Vn)

式中，每个子矩阵可如下计算：
Ui=∑j∈νi(J~ijc)⊤J~ijcU_i=\sum_{j\in\nu _i}^{}\left ( \widetilde J_{ij}^c \right )^\top\widetilde J_{ij}^cUi=j∈νi∑(Jijc)⊤JijcVj=∑i∈{k∣j∈νk}(J~ijx)⊤J~ijxV_j=\sum_{i\in\left \{ k|j\in \nu_k \right \}}^{}\left ( \widetilde J_{ij}^x \right )^\top\widetilde J_{ij}^xVj=i∈{k∣j∈νk}∑(Jijx)⊤Jijx

式中，νi\nu_iνi为相机iii中可见的三维点集合，相应地，有：
u=(u1⊤⋯um⊤)⊤u=(u_1^\top\cdots u_m^\top)^\topu=(u1⊤⋯um⊤)⊤v=(v1⊤⋯vn⊤)⊤v=(v_1^\top\cdots v_n^\top)^\topv=(v1⊤⋯vn⊤)⊤

式中，每个子向量可由下式计算：
ui=−∑j∈νi(J~ijc)⊤e~iju_i=-\sum_{j\in \nu _i}\left ( \widetilde J_{ij}^c \right )^\top\widetilde e_{ij}ui=−j∈νi∑(Jijc)⊤eij Vj=−∑i∈{k∣j∈νk}(J~ijx)⊤e~ijV_j=-\sum_{i\in\left \{ k|j\in \nu_k \right \}}^{}\left ( \widetilde J_{ij}^x \right )^\top\widetilde e_{ij}Vj=−i∈{k∣j∈νk}∑(Jijx)⊤eij

WWW为m×nm\times nm×n的块矩阵：
W=[W11⋯W1n⋮⋱⋮Wm1⋯Wmn]W=\begin{bmatrix} W_{11} & \cdots & W_{1n}\\ \vdots & \ddots & \vdots\\ W_{m1} & \cdots & W_{mn} \end{bmatrix}W=⎣⎢⎡W11⋮Wm1⋯⋱⋯W1n⋮Wmn⎦⎥⎤

式中，每个子矩阵为Wij=(J~ijc)⊤J~ijxW_{ij}=\left ( \widetilde J_{ij}^c \right )^\top\widetilde J_{ij}^xWij=(Jijc)⊤Jijx。

WWW也是个稀疏矩阵，当且仅当三维点jjj在相机iii中可见时Wij≠0W_{ij}\neq 0Wij=0。总结来说，由上述一系列推导可以显式地利用集束调整特殊的稀疏特性，高效构造正规方程。

由于三维点数nnn通常较大，一个较小尺度的场景往往也有几千甚至几万个三维点，导致正规方程很庞大，不宜直接求解Δφ\Delta \varphiΔφ。集束调整算法进一步利用正规方程的稀疏特性提高Δφ\Delta \varphiΔφ的求解效率。

由于三维点数n通常远大于图像帧数m，可先对所有三维点进行边缘化（marginalization），得到一个仅关于相机变量Δc\Delta cΔc的较小线性系统。求解较小线性系统得到Δc\Delta cΔc后，再代入正规方程中求解三维点变量Δx\Delta xΔx。

具体来说，对正规方程等式两边左乘如下矩阵：
[Im×m−WV−10m×nIn×n]\begin{bmatrix} I_{m\times m} & -WV^{-1}\\ 0_{m\times n} & I_{n\times n} \end{bmatrix}[Im×m0m×n−WV−1In×n]

得到：
[U−WV−1W⊤0W⊤V][ΔcΔx]=[u−WV−1vv]\begin{bmatrix} U-WV^{-1}W^\top & 0\\ W^\top & V \end{bmatrix}\begin{bmatrix} \Delta c\\ \Delta x \end{bmatrix}=\begin{bmatrix} u-WV^{-1}v\\ v \end{bmatrix}[U−WV−1W⊤W⊤0V][ΔcΔx]=[u−WV−1vv]

由于上式系数矩阵的右上角元素为0，相机变量Δc\Delta cΔc的计算可以独立于三维点变量Δx\Delta xΔx通过求解下式得到：
SΔc=gS\Delta c=gSΔc=g
式中，S=U−WV−1W⊤S=U-WV^{-1}W^\topS=U−WV−1W⊤，g=u−WV−1vg=u-WV^{-1}vg=u−WV−1v。

式SΔc=gS\Delta c=gSΔc=g称为舒尔补方程，舒尔补方程中的SSS，ggg可利用U,V,WU,V,WU,V,W特殊的稀疏特性高效构造。具体来说，将SSS看作一个m×mm\times mm×m的块矩阵：
S=[S11⋯S1m⋮⋱⋮Sm1⋯Smm]S=\begin{bmatrix} S_{11} & \cdots & S_{1m}\\ \vdots & \ddots & \vdots\\ S_{m1} & \cdots & S_{mm} \end{bmatrix}S=⎣⎢⎡S11⋮Sm1⋯⋱⋯S1m⋮Smm⎦⎥⎤

对于对角线上的子矩阵：
Sii=Uii−∑j∈νiWijVj−1Wij⊤S_{ii}=U_{ii}-\sum_{j\in \nu_i}W_{ij}V_j^{-1}W_{ij}^\topSii=Uii−j∈νi∑WijVj−1Wij⊤

非对角线上的子矩阵：
Si1i2=−∑j∈νi1⋂νi2Wi1jVj−1Wi2j⊤S_{i_1i_2}=-\sum_{j\in \nu_{i_1}\bigcap\nu_{i_2}}W_{i_1j}V_j^{-1}W_{i_2j}^\topSi1i2=−j∈νi1⋂νi2∑Wi1jVj−1Wi2j⊤

相应地：
g=(g1⊤⋯gm⊤)⊤g=\begin{pmatrix} g_1^\top & \cdots & g_m^\top \end{pmatrix}^\topg=(g1⊤⋯gm⊤)⊤

式中，每个子向量：
gi=ui−∑j∈νiWijVj−1vij⊤g_{i}=u_{i}-\sum_{j\in \nu_i}W_{ij}V_j^{-1}v_{ij}^\topgi=ui−j∈νi∑WijVj−1vij⊤

由Si1i2S_{i_1i_2}Si1i2的计算式可知，当且仅当相机i1i_1i1和i2i_2i2存在公共点时Si1i2≠0S_{i_1i_2}\neq 0Si1i2=0，即SSS本身也是一个稀疏矩阵，可利用稀疏的线性系统求解算法，如AMD、PCG等算法高效求解相机变量Δc\Delta cΔc。最后，将Δc\Delta cΔc代入正规方程，解得三维点变量Δx\Delta xΔx。利用V,WV,WV,W的稀疏特性，每个三维点变量可以独立求解：
Δxj=Vj−1(vj−∑i∈{k∣j∈νk}Wij⊤Δci)\Delta x_j=V_j^{-1}(v_j-\sum_{i\in \left \{ k|j\in \nu _k \right \}}W_{ij}^\top\Delta c_i)Δxj=Vj−1(vj−i∈{k∣j∈νk}∑Wij⊤Δci)

以上是用高斯-牛顿法对集束调整求解的整个过程。虽然高斯牛顿法具有二阶收敛性，但是因为计算的J~⊤J~\widetilde J^\top \widetilde JJ⊤J信息矩阵不一定正定，而且每次迭代不一定能保证目标函数下降，所以容易存在不稳定问题。

在实际求解集束调整问题时，比较常采用LM（Levenberg-Marquart）算法。LM算法把牛顿法和梯度法结合起来，吸取了各自的优点，迭代的步长介于牛顿法和梯度法之间，并用参数λ\lambdaλ控制：当牛顿法收敛的时候，令λ\lambdaλ取较小的值，步长倾向于等于牛顿法步长；当λ\lambdaλ很大时，步长约等于梯度下降法的步长。这样既能保证收敛速度比较快，又能保证每次迭代目标函数是下降的。

但由于LM算法在每次迭代的时候对信息矩阵做了修改，导致无法进行增量式的计算。

位姿图优化

在集束调整中，三维点变量数一般会远大于相机变量数，导致求解的线性方程组规模非常巨大，即使利用稀疏性求解复杂度依然很高。位姿图优化于1997年被提出来以提高全局优化的效率。

在具体介绍位姿图之前，先介绍以下因子图。因子图是用于分析SfM/SLAM问题结构的一种常用工具。如图3.11所示。一般来说，SfM/SLAM问题中的变量和约束都会以节点形式存在，而因子图中的边则表示变量和约束之间的关联。由此可见，因子图是二分图，边只存在于变量节点和约束节点之间。

上图描绘了一个简单的SfM/SLAM问题的因子图示例，其中的红色节点代表状态变量，蓝色节点代表三维点变量，黑色节点代表变量之间的约束。

按照图示的例子，一个SfM/SLAM问题中，可能存在的约束有状态变量的先验ppp，相邻状态之间的相对位姿约束ν1⋯n\nu_{1\cdots n}ν1⋯n，非相邻状态之间的回路闭合约束l1l_1l1，l2l_2l2，以及视觉观测u1⋯8u_{1\cdots 8}u1⋯8等。求解完整集束调整的过程，就相当于最大化整个因子图中的所有约束节点的概率：
∏ifi(Xi)\prod_{i}f_i(X_i)i∏fi(Xi)

式中，XiX_iXi代表所有与因子fif_ifi相关的变量。如前所述，SfM/SLAM问题中，通常三维点变量的数量会远大于状态变量的数量，而通常一个状态变量可以观测到成百上千个三维点（一般称为地标点）。这就意味着即使状态变量以非常缓慢的速度增长，三维点的变量的增长也会是爆炸式的。显然，每次对所有变量进行估计是非常耗时的。

位姿图优化就是以因子图的形式对SfM/SLAM问题进行建模，同时在图中只保留表示位姿的状态节点，以及状态之间的相对位姿约束，而放弃了对数量庞大的三维点变量的持续优化，从而显著提升求解速度。

被舍弃的三维点变量会以相对位姿约束的形式保留下来。

一种构建相对位姿约束的方法是，在每次完成当前的集束调整步骤后，将当前的三维点变量“消去”，同时在形成共视关系的状态变量之间构建一系列相对位姿约束。

在删去被舍弃的三维点变量之前，要先以边缘化的方式将其信息保留下来，即得到剩余变量的边缘概率分布。因为消去三维点变量后，三维点变量的信息被“永远”定在了发生边缘化的时刻，而后续即使状态变量发生了变化，其与旧三维点之间的信息不能再改变。原因是这些信息已经被编码进了相对位姿约束。因此，位姿图优化其实是对集束调整的近似。如果三维点变量还没有收敛，那么可能会产生较大的误差，从而导致优化的结果不够理想。

增量式集束调整

由于三维点变量个数远远大于相机变量个数，所以在传统的集束调整中，正规方程的构造以及三维点的边缘化（舒尔补）的运算量，通常远远大于最后的相机变量求解步骤。

而在增量式的问题中，传统的集束调整包含很多冗余的运算，在每一次增加新的关键帧之后，原有正规方程中的许多变量几乎没有发生变化。由于篇幅较长，内容极其复杂，我直接写一些总结对比。

iSAM（Kaess et al., 2008）主要使用了增量更新平方根信息矩阵的策略，并且每隔一段时间会做一次完整的全局COLAMD消去变量和重新线性优化，本质上并不是完全增量的优化；而相比较而言，iSAM2（Kaess et al., 2012）将平方根据信息矩阵编码到贝叶斯树中，并使用贝叶斯树来深入分析变量之间的因果关系，在每次加入新的因子/变量的时候，选取影响到的一部分变量，使用COLMAD消去变量，然后增量更新线性化点，实现了完全增量的优化。

SLAM++、EIBA和ICE-BA都使用了增量式更新舒尔补的方法，实现增量式的集束调整。前者除了可以增量地优化变量以外，还可以非常快地恢复协方差矩阵。而EIBA和ICE-BA在实现上使用了块矩阵的结构，并可以通过记录影响矩阵的方式来减少计算。除此之外，SLAM++使用矩阵分解的方式来解线性方程，而EIBA/ICE-BA使用了基于块结构的预处理共轭梯度法来进行求解。

通常来说，基于块结构的预处理共轭梯度法在解舒尔补的时候，可以更充分地利用矩阵的块稀疏的结构；另外，在有比较好的初值的情况下，预处理共轭梯度法只需要较少的迭代次数就能收敛。

另外，与iSAM2相比，EIBA/ICE-BA会优先边缘化点来减少填充现象，使得仅仅只有可被观测到的点会被影响到（重新线性化）。尽管采用这一策略以后，所有相机变量都会被影响到，但iSAM2的策略却可能影响到很多在当前帧无法被观测到的三维点变量，使得这些变量都需要被重新线性化。由于三维点变量个数远远大于相机变量个数，所以EIBA/ICE-BA通常比iSAM2在增量式SfM和SLAM的集束调整问题中更为高效。