概述：该系统的输入是，一系列RGB-D图像，关键帧的初始位姿（通过SFM系统获得），通过插值关键帧，得到所有关键帧的初始位姿，然后相机位姿通过联合优化初始位姿和里程计位姿（ElasticFusion稠密跟踪获得），优化后的位姿用来最后的surfel重建。

Prior and Odometry Estimation

​ 估计关键帧位姿：对输入的RGB图，每隔$\lambda$ 帧选取一帧作为关键帧，然后通过目前最先进的算法COLMAP来估计它的初始位姿以及对应的协方差矩阵。参考[1],[2]。

​ 估计里程计位姿：使用ElasticFusion来估计帧i,j之间的相对位姿。

Factor Graph Representation

​ 这里是系统的核心，采用作者提出的因子图公式，联合优化关键帧的初始位姿和里程计位姿。

Pose Initialization: Interpolation on SE(3) Manifold

​ 使用[25]来初始化两个关键帧之间的位姿。首先计算两个关键帧位姿的差值，然后映射到se(3)上$\delta \xi$，以满足线性操作。最后估计第k帧的位姿$P_k$，$P_i$是上一个关键帧的位姿。

$$P_k=P_{i}exp(\frac{k-i}{\lambda }\delta \xi )$$

Scale Initialization

​ 由于SFM估计的位姿有尺度不确定性，所以需要找到一个尺度因子去对齐这两组位姿。对于一组已知的全局坐标的3D路标点$X_{sfm}$以及这些路标在第一帧里的像素坐标，然后通过深度信息，计算出路标像素点对应的3D坐标$X_{slam}$，通过icp使用SVD分解估计两组3D点的变换矩阵。

Optimization and Surfel-based Model Reconstruction

​ 当获得新的里程计位姿，将其插入到因子图中，使用iSAM2进行增量优化[27]。通过全局一致的先验位姿约束里程计位姿。