前言
光束法平差Bundle Adjustment，据说是三维重建领域的标配算法，然而我懂不了一点，因此专门开一个博客予以记录。本文主要参考了[1] CSDN博客 。
光束法平差
光束法平差是一种优化框架，其主要思路是如何围绕重投影误差 ，使用梯度下降法、牛顿迭代法、LM等方法完成优化。自从2000年论文[1] Reprojection Error 打交道，以该误差函数作为待优化的目标函数，将是一个典型的非线性优化问题。
三维点匹配
借用博客 提供的图：如何重建出全部关键点的空间坐标，并解算出每个摄像机的位姿？
优化目标函数
该问题可以建模为优化投影过程的目标函数，如果设空间关键点的三维坐标是X i , ( i = 1 , 2 , ⋯ , N ) \mathbf{X}_i,(i=1,2,\cdots,N) X i , ( i = 1 , 2 , ⋯ , N ) T j , ( j = 1 , 2 , ⋯ , M ) \mathbf{T}_j,(j=1,2,\cdots,M) T j , ( j = 1 , 2 , ⋯ , M ) K j \mathbf{K}_j K j R j \mathbf{R}_j R j
min X i , T j , R j ∑ i = 1 N ∑ j = 1 M w i j ⋅ ∥ x i , j − K j ( R j X i + T j ) ∥ 2 \begin{aligned}
\min_{\mathbf{X}_i,\mathbf{T}_j,\mathbf{R}_j} \sum_{i=1}^N \sum_{j=1}^M w_{ij}\cdot\left\| \mathbf{x}_{i,j} - \mathbf{K}_j \left( \mathbf{R}_j \mathbf{X}_i + \mathbf{T}_j \right) \right\|^2
\end{aligned}
X i , T j , R j min i = 1 ∑ N j = 1 ∑ M w ij ⋅ ∥ x i , j − K j ( R j X i + T j ) ∥ 2
式中的下标i i i j j j w i j w_{ij} w ij K j \mathbf{K}_j K j [1]
min X , T j , R j ∑ j 1 2 ( z j − x j ) ⊤ W j ( z j − x j ) \min_{\mathbf{X},\mathbf{T}_j,\mathbf{R}_j}\sum_j \frac{1}{2}\mathbf{(z_\mathit{j}-x_\mathit{j})^\top W_\mathit{j}(z_\mathit{j}-x_\mathit{j})}
X , T j , R j min j ∑ 2 1 ( z j − x j ) ⊤ W j ( z j − x j )
其中的z j = K ( R X j + T ) \mathbf{z}_j=\mathbf{K(RX_\mathit{j}+T)} z j = K ( R X j + T ) W \mathbf{W} W R j , T j \mathbf{R}_j,\mathbf{T}_j R j , T j
min X , T j , R j ∑ j ρ j [ 1 2 ( z j − x j ) ⊤ W j ( z j − x j ) ] \min_{\mathbf{X},\mathbf{T}_j,\mathbf{R}_j}\sum_j \rho_j\left[\frac{1}{2}\mathbf{(z_\mathit{j}-x_\mathit{j})^\top W_\mathit{j}(z_\mathit{j}-x_\mathit{j})}\right]
X , T j , R j min j ∑ ρ j [ 2 1 ( z j − x j ) ⊤ W j ( z j − x j ) ]
其中的ρ j \rho_j ρ j
ρ j ( x ) = − log [ exp ( χ i p 2 ) + ϵ ] \rho_j(x)=-\log\left[\exp\left(\chi_{ip}^2\right)+\epsilon\right]
ρ j ( x ) = − log [ exp ( χ i p 2 ) + ϵ ]
其中的χ i p 2 = 1 2 ( z j − x j ) ⊤ W j ( z j − x j ) \displaystyle\chi_{ip}^2=\frac{1}{2}\mathbf{(z_\mathit{j}-x_\mathit{j})^\top W_\mathit{j}(z_\mathit{j}-x_\mathit{j})} χ i p 2 = 2 1 ( z j − x j ) ⊤ W j ( z j − x j )
求解方法
论文[1]
牛顿迭代法
牛顿迭代法收敛速度快，但是计算量大，需要计算目标函数的Hessian矩阵，将目标函数作二阶Taylor展开可得：
f ( x + Δ x ) ≈ f ( x ) + ∇ f ( x ) Δ x + 1 2 Δ x ⊤ ∇ 2 f ( x ) Δ x f(\mathbf{x}+\Delta\mathbf{x})\approx f(\mathbf{x})+\nabla f(\mathbf{x})\Delta\mathbf{x}+\frac{1}{2}\Delta\mathbf{x}^\top\nabla^2f(\mathbf{x})\Delta\mathbf{x}
f ( x + Δ x ) ≈ f ( x ) + ∇ f ( x ) Δ x + 2 1 Δ x ⊤ ∇ 2 f ( x ) Δ x
其中可记g = ∇ f ( x ) \mathbf{g}=\nabla f(x) g = ∇ f ( x ) H = ∇ 2 f ( x ) \mathbf{H}=\nabla^2 f(x) H = ∇ 2 f ( x )
d f d x ( x + Δ x ) ≈ H Δ x + g = 0 ⟹ Δ x = − H − 1 g \frac{\mathrm{d}f}{\mathrm{d}\mathbf{x}}(\mathbf{x+\Delta x})\approx \mathbf{H\Delta x+g}=0\implies \mathbf{\Delta x=-H^{-1}g}
d x d f ( x + Δx ) ≈ HΔx + g = 0 ⟹ Δx = − H − 1 g
然后完成对参数z \mathbf{z} z
z ← z − H − 1 g \mathbf{z}\leftarrow\mathbf{z}-\mathbf{H}^{-1}\mathbf{g}
z ← z − H − 1 g
Levenberg-Marquardt方法
LM法是一种结合了高斯牛顿法和梯度下降法的方法，在牛顿迭代法的基础上，增加一项调节因子λ \lambda λ
Δ x = − ( H + λ W ) − 1 g \mathbf{\Delta x}=-\left(\mathbf{H}+\lambda\mathbf{W}\right)^{-1}\mathbf{g}
Δx = − ( H + λ W ) − 1 g
该方法可以自主根据当前下降趋势调节λ \lambda λ
高斯牛顿法
以上提到的两个方法都要计算Heissan矩阵，该步过程实际上计算量较大，实时性算法中往往不能满足。高斯牛顿法是一种近似方法，它认为最优解就在初始值的附近，因此可以使用观测值z \mathbf{z} z x \mathbf{x} x
{ g = d f d x = d f d z d z d x = ( z − x ) ⊤ W J H = d 2 f d x 2 = d z d x d 2 f d z 2 d z d x = J ⊤ W J + ∑ j ( z i − x i ) W i d 2 z i d x 2 \left\{
\begin{aligned}
\mathbf{g}&=\frac{\mathrm{d}f}{\mathrm{d}\mathbf{x}}=\frac{\mathrm{d}f}{\mathrm{d}\mathbf{z}}\frac{\mathrm{d}\mathbf{z}}{\mathrm{d}\mathbf{x}}=\mathbf{(z-x)^\top WJ}\\
\mathbf{H}&=\frac{\mathrm{d}^2f}{\mathrm{d}\mathbf{x}^2}=\frac{\mathrm{d}\mathbf{z}}{\mathrm{d}\mathbf{x}}\frac{\mathrm{d}^2f}{\mathrm{d}\mathbf{z}^2}\frac{\mathrm{d}\mathbf{z}}{\mathrm{d}\mathbf{x}}=\mathbf{J^\top WJ}+\sum_j\mathbf{(z_i-x_i)W_i\frac{\mathrm{d}^2\mathbf{z}_i}{\mathrm{d}\mathbf{x}^2}}
\end{aligned}
\right.
⎩ ⎨ ⎧ g H = d x d f = d z d f d x d z = ( z − x ) ⊤ WJ = d x 2 d 2 f = d x d z d z 2 d 2 f d x d z = J ⊤ WJ + j ∑ ( z i − x i ) W i d x 2 d 2 z i
而后一项在小邻域内往往是线性的，因此可以认为d 2 z i d x 2 ≈ 0 \displaystyle\frac{\mathrm{d}^2\mathbf{z}_i}{\mathrm{d}\mathbf{x}^2}\approx 0 d x 2 d 2 z i ≈ 0
Δ x = − ( J ⊤ W J ) − 1 J ⊤ W ( z − x ) \mathbf{\Delta x}=-\left(\mathbf{J^\top WJ}\right)^{-1}\mathbf{J^\top W(z-x)}
Δx = − ( J ⊤ WJ ) − 1 J ⊤ W ( z − x )
对于使用带有ρ j \rho_j ρ j
{ g j = r h o j ′ J j ⊤ W j J j ( z − x ) j H j = J j ⊤ [ ρ j ′ W j + 2 ρ j ′ ′ ( W j ( z − x ) j ) ( W j ( z − x ) j ) ⊤ ] J j \left\{
\begin{aligned}
\mathbf{g}_j&=rho_j'\mathbf{J}_\mathit{j}^\top \mathbf{W}_j\mathbf{J}_j\mathbf{(z-x)}_j\\
\mathbf{H}_j&=\mathbf{J}_j^\top\left[\rho_j'\mathbf{W}_j+2\rho''_j(\mathbf{W}_j\mathbf{(z-x)}_j)(\mathbf{W}_j\mathbf{(z-x)}_j)^\top\right]\mathbf{J}_j
\end{aligned}
\right.
⎩ ⎨ ⎧ g j H j = r h o j ′ J j ⊤ W j J j ( z − x ) j = J j ⊤ [ ρ j ′ W j + 2 ρ j ′′ ( W j ( z − x ) j ) ( W j ( z − x ) j ) ⊤ ] J j
求解过程
BA算法是典型的非线性优化，因此不要企图得到解析解能够一步完成全部结果的计算，最常用的方式是增量式优化incremental 。详细的过程可以参考论文[2]
任取两张图像，根据匹配的关键点计算这两幅图像的本质矩阵，并以此分解出两个相机的位姿信息 （包括姿态矩阵R j \mathbf{R}_j R j T j \mathbf{T}_j T j 三维重建结果 ，即为初始结果；
以第一幅图像得到的三维点为基准，加入第三个摄像机取得的图像关键点，由三维点到图像平面可以直接求出第三个摄像机的位姿信息（此时有误差产生，因为双目三角化计算得到的空间点不是准确的）；
逐步增加相机图像，误差累计，选取合适的频数，对全部摄像机图像作一次BA优化，初始值就可以选为三角化方法得到的结果；
重复1-3步骤，直到遍历完全部摄像机的图像，然后对最后遍历结果再作一次BA优化，得到最终的结果。
通过以上的方式可以从任意数量的不同视角图像中完成较精确的三维点重建恢复。
常用的数值计算库
实际使用中，推荐使用g2o库，该库作为基础构件，应用于包括COLMAP在内的多个开源软件，接口丰富计算效率较高，目前也提供了python接口（但是文档并不完善）。
参考文献
