Factor Graph, 因子图

引言

因子图方法广泛应用于机器人姿态估计，多种信号融合领域，在机器视觉、无人机导航、无人驾驶领域应用广泛，与传统卡尔曼滤波方式相比，能提供更高精度，更平滑的全局结果。 因子图本质上是贝叶斯网络最大后验估计，数学上转化成对一个全局目标函数进行优化，全局函数由多个小因子(约束)构成，一般目标函数是待优化变量的非线性函数，在优化过程中需要变量当前位置进行线性化，利用Gauss-Netwon或其他方式迭代逼近最优解。主要计算步骤：残差计算、线性化、线性方程求解器、优化变量更新。当系统规模很大情况下，利用处理平台上多核CPU或GPU加速卡，可以对优化结算过程进行加速优化。

优化策略和实现

针对多核CPU处理器，采用OpenMP多线程编程模式进行并行计算。OpenMP适合对循环在线程中间进行分割划分，并行执行计算，同时提供锁、临界区等功能。OpenMP可以在对串行程序进行较小改动情况下，能较充分利用多核计算能力。因子图计算中的一些步骤可以直接使用OpenMP进行优化，一些步骤需要考虑线程间的互相影响，一个策略是采用临界区保证运行正确，另一个策略是对输入数据进行逻辑划分，让线程间的运算尽可能没有互相影响。

针对NVidia 公司GPU加速卡，使用CUDA编程将因子图计算中适合步骤迁移到GPU卡上进行计算，一些适合CPU的步骤放在主机端进行，CPU和GPU通过数据异步传输，进行中间结果交换，实现异构计算并行优化

残差计算并行优化

残差计算在CPU或GPU上并行计算比较直接。每个因子可以作为一个原子任务，分配给CPU线程或是GPU线程块进行计算，最后进行求和汇总。残差计算因计算量不大，比较适合在CPU端进行计算

目标函数线性化并行优化

线性化过程包含两个步骤。第一步是因子图中的每个因子进行线性化，获得对应Jacobian矩阵，因子间的计算独立，可以直接映射到CPU线程或GPU计算单元上；第二步是用每个因子的Jacobian小矩阵构造线性方程组的Hessian矩阵，本步骤并行需要根据图的拓扑结构进行划分，把因子图划分成多个子图，每个子图内的因子对全局Hessian矩阵的更新没有关联，可以并行独立计算。

线性方程求解器并行优化

Hessian矩阵一般是稀疏块状矩阵。针对特定问题可以采用Domain Decomposition方式进行并行求解(Kalman滤波应用场景)，或是利用Schur Complement进行消元降维(Bundle Adjustment问题)。利用稀疏矩阵Cholesky直接求解或是PCG(Preconditioned Conjugate Gradient)迭代求解。可利用现有库进行计算，比如Magma、cuSolver等。

变量并行更新

变量更新之间不相关。可以直接在CPU线程或GPU计算单元(WARP或线程块)进行并行计算。 因计算量较小，此步骤适合放在CPU端进行

开源参考资料

• GTSAM
• MINISAM
• CERES