网络资源
- 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 V_i Vi相当于 ∑ J p T ∗ J p \sum{J_p}^T*J_p ∑JpT∗Jp, W i W_i Wi相当于 { 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 − ∑ W i V i − 1 W i T S=U-WV^{-1}W^T=U-\sum{W_iV_i^{-1}W_i^T} S=U−WV−1WT=U−∑WiVi−1WiT对 V i V_i Vi进行 L L T LLT LLT分解 V i = L i ∗ L i T V_i=L_i*L_i^T Vi=Li∗LiT变为 S = U − ∑ ( L i − 1 W i ) T ( L i − 1 W i ) S=U-\sum{(L_i^{-1}W_i)^T(L_i^{-1}W_i)} S=U−∑(Li−1Wi)T(Li−1Wi)并行计算策略,此处不能简单对空间点并行划分,因可能有多个点对矩阵 S S S的相同子块进行更新,这里采用对 S S S的子块进行并行更新,对于某个子块 S i , j S_{i,j} Si,j, 需要遍历所有空间点,如果空间点所包含的测量同时含有 i , j i,j i,j相机,那么本空间点就会对子块 S i , j S_{i,j} Si,j有贡献, S S S的各个子块之间更新不相关,可并行处理,更适合GPU上计算(CPU上需要三重循环,所有空间点的遍历位于最里面,增加较多冗余计算,对于姿态较少问题,采用遍历所有空间点的串行方式效率更高)
- RCS矩阵方程右手边向量 η r h s = ϵ a − ∑ W i V i − 1 ϵ b , i = U − ∑ ( L i − 1 W i ) T ( L i − 1 ϵ b , i ) \eta_{rhs}=\epsilon_a-\sum{W_iV_i^{-1}\epsilon_{b,i}}=U-\sum{(L_i^{-1}W_i)^T(L_i^{-1}\epsilon_{b,i})} ηrhs=ϵa−∑WiVi−1ϵb,i=U−∑(Li−1Wi)T(Li−1ϵb,i)所以计算RCS方程,只需要计算 ( L i − 1 W i ) (L_i^{-1}W_i) (Li−1Wi)和 ( L i − 1 ϵ b , i ) (L_i^{-1}\epsilon_{b,i}) (Li−1ϵb,i),可以并行计算
- RCS方程 S ∗ δ a = η r h s S*\delta_a=\eta_{rhs} S∗δa=ηrhs进而计算出 δ b = V − 1 ( ϵ b − W ∗ δ a ) \delta_b=V^{-1}(\epsilon_b-W*\delta_a) δb=V−1(ϵb−W∗δa)根据 V V V的性质,得到 δ b , i = L i − T ( ( L i − 1 ϵ b , i ) − ( L i − 1 W i ) ∗ δ a ) \delta_{b,i}=L_i^{-T}((L_i^{-1}\epsilon_{b,i})-(L_i^{-1}W_i)*\delta_a) δb,i=Li−T((Li−1ϵb,i)−(Li−1Wi)∗δa)其中 V i V_i Vi和 L i L_i Li都是 3 × 3 3\times3 3×3矩阵, ϵ b , i \epsilon_{b,i} ϵb,i和 δ b , i \delta_{b,i} δb,i都是 3 × 1 3\times1 3×1向量, 每个 W i W_i Wi都是块状稀疏矩阵,每一块大小 3 × N c 3\times Nc 3×Nc
RCS方程 S ∗ δ a = η r h s S*\delta_a=\eta_{rhs} S∗δa=ηrhs求解策略
- 直接用Cholesky分解 S S S. 对于稠密问题用稠密分解方式,对于大规模稀疏问题用稀疏分解(参考Eigen库),在分解前可用Ordering methods (AMD, CAMD, COLAMD, and CCOLAMD)进行Reordering,提高分解效率。矩阵 S S S的Sparsity Pattern是固定的,Reordering只需要计算一次。
- PCG(Preconditioned Conjugate Gradient)迭代求解器。 M − 1 S ∗ δ a = M − 1 η r h s M^{-1}S*\delta_a=M^{-1}\eta_{rhs} M−1S∗δa=M−1ηrhs需要提供Preconditioner矩阵 M − 1 M^{-1} M−1, 可以取 S S S的对角块,还需要提供 S x Sx Sx矩阵向量计算,依据 S S S的形式,不需要显式构造出 S S S, 利用 S x = U x − ∑ W i V i − 1 W i T x Sx=Ux-\sum{W_iV_i^{-1}W_i^T}x Sx=Ux−∑WiVi−1WiTx可多线程并行(CPU上可采用OpenMP多线程形式,GPU上按线程块划分计算)
性能比较
- 依据minisam中的BA例子。49个相机姿态,7000多个点,3万多个观测
- minisam实现: factor graph
- RCS直接Cholesky分解。针对本问题RCS矩阵 S S S是小规模接近稠密矩阵,主要耗时发生在线性化和 S S S构造上,基本耗时 T t o t a l = T l i n e a r + T s + T l u + T δ b T_{total}=T_{linear}+T_{s}+T_{lu}+T_{\delta_b} Ttotal=Tlinear+Ts+Tlu+Tδb
- PCG。 多次迭代(100次左右),主要耗时发生在 S x Sx Sx计算上,求解 M − 1 x M^{-1}x M−1x较为简单高效. 可采用较为复杂矩阵 M M M形式(比如取 S S S的块状三对角矩阵),提高收敛性,或 M M M可采用更大的对角块! 基本耗时 T t o t a l = T l i n e a r + N i t r × T p c g T_{total}=T_{linear}+N_{itr}\times T_{pcg} Ttotal=Tlinear+Nitr×Tpcg 其中 T p c g ≈ T M y = b + T S x ≈ T S x T_{pcg}\approx T_{My=b}+T_{Sx} \approx T_{Sx} Tpcg≈TMy=b+TSx≈TSx