Bundle-Adjustment并行求解器

网络资源

• Bundle Adjustment on the GPU
• Siddharth Choudhary主页
• Multicore Bundle Adjustment
• BA笔记
• Parralel Bundle Adjustment
• RPBA, Robust Parallel Bundle Adjustment
• ceres solver学习之bundle adjustment
• Tutorial BA
• ORB_SLAM2学习之源码分析三-优化
• Bundle Adjustment简述
• sba : A Generic Sparse Bundle Adjustment C/C++ Package Based on the Levenberg-Marquardt Algorithm
• Gaussian Belief Propagation for Bundle Adjustment and SLAM
• Poplar Implementation of Bundle Adjustment using Gaussian Belief Propagation on Graphcore’s IPU
• HKUST, Lei Zhou (周磊)
• visualSFM : A Visual Structure from Motion System
• Clustering Views for Multi-view Stereo (CMVS)
• 三维重建系统

矩阵结构

优化器设计

采用LevenbergMarquardt优化器，主要计算部分在目标函数线性化和RCS(Reduced Cameras System)矩阵构建和求解, 等同于Schur Complement步骤，先求解姿态修正量，之后求解空间点的修正量。首先对所有投影测量排序，第一关键字是空间点下标，第二关键字是相机下标，并确定每个空间点在排序后的整体测量中对应的起始和结束位置(相当于构建Visibility矩阵, 空间点看相机)。

• 对所有空间点并行进行残差和Jacobian计算 { e , J p , J c } \{e,J_p,J_c\} {e,Jp​,Jc​}，每个点对应的 { V i , W i } \{V_i,W_i\} {Vi​,Wi​}可以并行计算, $V_i$相当于$\sum {J_p}^T\ast J_p$， $W_i$相当于 { J p T ∗ J c } \{J_p^T*J_c\} {JpT​∗Jc​}横向stack在一起 ，矩阵U是块状对角阵，只需要保存对角块即可，每个线程有私有U, 线程只更新本线程U矩阵，当计算遍历完所有空间点后，会将每个线程私有U更新到全局U矩阵上(使用omp critical section临界区规约求和)
• RCS矩阵计算$$S=U-W{V}^{-1}{W}^T=U-\sum W_i{V}_i^{-1}{W}_i^T$$对$V_i$进行$LLT$分解$$V_i=L_i\ast L_i^T$$变为$$S=U-\sum (L_i^{-1}{W}_i{)}^T(L_i^{-1}{W}_i)$$并行计算策略，此处不能简单对空间点并行划分，因可能有多个点对矩阵$S$的相同子块进行更新，这里采用对$S$的子块进行并行更新，对于某个子块$S_{i,j}$, 需要遍历所有空间点，如果空间点所包含的测量同时含有$i,j$相机，那么本空间点就会对子块$S_{i,j}$有贡献，$S$的各个子块之间更新不相关，可并行处理，更适合GPU上计算(CPU上需要三重循环，所有空间点的遍历位于最里面，增加较多冗余计算，对于姿态较少问题，采用遍历所有空间点的串行方式效率更高)
• RCS矩阵方程右手边向量$${\eta }_{rhs}={ϵ}_a-\sum W_i{V}_i^{-1}{ϵ}_{b,i}=U-\sum (L_i^{-1}{W}_i{)}^T(L_i^{-1}{ϵ}_{b,i})$$所以计算RCS方程，只需要计算$(L_i^{-1}{W}_i)$和$(L_i^{-1}{ϵ}_{b,i})$,可以并行计算
• RCS方程$$S\ast {\delta }_a={\eta }_{rhs}$$进而计算出$${\delta }_b=V^{-1}({ϵ}_b-W\ast {\delta }_a)$$根据$V$的性质，得到$${\delta }_{b,i}=L_i^{-T}((L_i^{-1}{ϵ}_{b,i})-(L_i^{-1}{W}_i)\ast {\delta }_a)$$其中$V_i$和$L_i$都是$3\times 3$矩阵，${ϵ}_{b,i}$和${\delta }_{b,i}$都是$3\times 1$向量， 每个$W_i$都是块状稀疏矩阵，每一块大小$3\times Nc$

RCS方程$S\ast {\delta }_a={\eta }_{rhs}$求解策略

• 直接用Cholesky分解$S$. 对于稠密问题用稠密分解方式，对于大规模稀疏问题用稀疏分解(参考Eigen库)，在分解前可用Ordering methods (AMD, CAMD, COLAMD, and CCOLAMD)进行Reordering,提高分解效率。矩阵$S$的Sparsity Pattern是固定的，Reordering只需要计算一次。
• PCG(Preconditioned Conjugate Gradient)迭代求解器。$$M^{-1}S\ast {\delta }_a=M^{-1}{\eta }_{rhs}$$需要提供Preconditioner矩阵$M^{-1}$, 可以取$S$的对角块，还需要提供$Sx$矩阵向量计算，依据$S$的形式，不需要显式构造出$S$, 利用$Sx=Ux-\sum W_i{V}_i^{-1}{W}_i^{T}x$可多线程并行(CPU上可采用OpenMP多线程形式，GPU上按线程块划分计算)

性能比较

• 依据minisam中的BA例子。49个相机姿态，7000多个点，3万多个观测
• minisam实现: factor graph
• RCS直接Cholesky分解。针对本问题RCS矩阵$S$是小规模接近稠密矩阵,主要耗时发生在线性化和$S$构造上，基本耗时$$T_{total}=T_{linear}+T_s+T_{lu}+T_{{\delta }_b}$$
• PCG。 多次迭代(100次左右)，主要耗时发生在$Sx$计算上，求解$M^{-1}x$较为简单高效. 可采用较为复杂矩阵$M$形式(比如取$S$的块状三对角矩阵)，提高收敛性，或 $M$可采用更大的对角块! 基本耗时$$T_{total}=T_{linear}+N_{itr}\times T_{pcg}$$ 其中 $$T_{pcg}\approx T_{My=b}+T_{Sx}\approx T_{Sx}$$