因子图：全局优化位姿

SLAM（Simultaneous Localization and Mapping，同时定位与建图）中的因子图是一种数学表达形式，用于描述机器人或无人驾驶车辆在未知环境中运动时产生的所有观测数据以及它们之间的相对关系。因子图是概率图模型的一种，特别适用于表示SLAM问题中的不确定性及依赖关系。

简而言之，因子图就是考虑每一次的状态量和观测量之间的残差，使用非线性优化每一次状态量，类似回环检测般，对每一次的位姿进行优化。

在SLAM中，因子图的作用主要包括以下几点：

1. 全局一致性：

   • 因子图能够将局部观测数据（如里程计读数、视觉特征匹配等）作为因子节点加入到图中，每个因子代表一种测量或约束条件。
   • 这些因子连接着地图中的路标点（landmarks）和机器人的位姿（pose），形成一个全局一致性的框架，从而考虑所有观测的整体一致性，而不是孤立地处理单个测量。

2. 优化框架：

   • 利用因子图可以将SLAM问题转化为非线性优化问题，通过最小化整个图的能量函数（通常表现为所有因子的残差平方和）来估计最优的地图和机器人的轨迹。
   • 通过图优化算法（如Gauss-Newton, Levenberg-Marquardt, iSAM系列算法等）迭代求解，能够得到全局最优或者近似全局最优的解决方案。

3. 增量式更新与维护：

   • 因子图支持增量式更新，随着新的观测数据到来，可以添加新的因子并重新优化整个图，无需从头开始计算，大大提高了实时性和计算效率。
   • 对于部分优化方法，比如ISAM（Incremental Smoothing and Mapping），可以在保持图结构的同时，仅针对变化的部分进行有效优化。

4. 便于处理不确定性：

   • 因子图能很好地表示各个观测值的不确定性，每个因子都附带相应的协方差矩阵，描述了观测噪声的程度，这在处理具有不确定性的传感器数据时非常关键。

5. 灵活扩展：

   • 因子图结构可以很容易地适应不同类型的传感器数据和约束类型，无论是视觉、激光雷达还是其他传感器的数据，都可以以因子的形式纳入图中进行统一优化。

因此，因子图作为一种强有力的工具，对于解决SLAM中的复杂优化问题具有显著的优势。