视觉SLAM笔记（55） 位姿图
视觉 SLAM 中的位姿图（Pose Graph）是一种通过优化相机位姿之间的相对运动关系来解决定位与建图问题的方法。其核心思想是将相机位姿作为节点，并通过相对运动约束作为边构建一个图结构。由于特征点在优化问题中占主导地位，但其位置估计趋于稳定，因此可以固定这些特征点的位置，仅对相机位姿进行优化以提高效率。位姿图的优化通常采用李群和李代数框架，并结合最小二乘法求解误差函数的最优解。通过这种方法，在实时性和计算效率之间实现了平衡，并广泛应用于 SLAM 应用场景中。
视觉SLAM笔记（55） 位姿图
- 1. Pose Graph 的意义
- 2. Pose Graph 的优化
1. Pose Graph 的意义
该图形优化问题被称为Bundle Adjustment（BA）算法，并且能够有效地解决大规模定位与建图问题。然而，在技术发展的推动下，机器人运动轨迹会愈发延伸漫长，并带动地图规模不断扩大。类似Bundle Adjustment（BA）这类方法，在计算效率上也会表现出逐渐下降的趋势。
在之前的分析中指出，在优化问题中占据主导地位的关键性特征点数量显著。通过持续观察，在多次测量后动态定位结果趋于稳定。远处未稳定的异常数据逐渐消失。进一步优化这些稳定的数据会显得徒劳无益。
因此，在进行了几次优化后就将其固定下来，并不将其视为位姿估计的目标而仅作为约束条件处理
按照这一思路继续深入探讨时会发现：是否可以完全摒弃路标管理而专注于轨迹追踪？
可以构建一个仅基于轨迹的优化模型
值得注意的是，在这种情况下
这种方法使得我们只需关注关键帧间的相对运动关系而不必处理路标优化问题
通过这种方式我们成功地实现了对大量特征点优化计算量的有效缩减
从而形成了著名的位姿图（Pose Graph）
在 BA 方法中，“关键帧”的数量显著多于机器人位姿估计中的关键节点数量
- 要么借鉴滑动窗口法的精神，在处理过程中舍弃一些历史数据
- 要么参考Pose Graph的做法，在不进行路径重优化的前提下仅利用poses之间的几何关系
2. Pose Graph 的优化
在Pose Graph图优化问题中涉及到了节点与边。其中每个节点代表相机的位姿信息，并用变量ξ₁,…,ξₙ来描述。边上记录的是两个相邻位姿之间的相对运动估计。这一估计通常采用特征点匹配法或直接的方法进行计算。无论采用何种方法进行计算，在实际应用中我们通常会得到一个具体的运动量∆ξ_ij。这个运动量的不同表达形式在不同算法中可能会有所差异：取比较自然的一种：
或按李群的写法：
从图形化优化的角度分析，在实际应用中这一等式通常无法完全精确成立。因此，在此采用最小二乘误差作为评价指标，并类似于传统优化方法的做法进行求解。接着为了避免复杂的计算过程，在对误差函数求取关于优化变量的导数后完成进一步的迭代运算。
这里，把上式的 ∆Tij 移至等式右侧，构建误差 eij：
在优化过程中涉及两个变量：ξi 和 ξj。进而需要计算 eij 对这两个变量的导数。按照李代数的求导规则，在 ξi 和 ξj 上分别施加小扰动 δξi 和 δξj。误差由此发生改变：
该式中居于中间位置的是两个扰动项。
为了应用BCH近似法，在式子左侧或右侧转移它们。
目前将它作为先验知识处理。
稍加改变，有：
该式揭示了借助引入一个伴随项的方式，在T左右两侧调换扰动项的位置。利用这一技巧，则可以使被扰动移动至右侧（当然也可以置于左侧），从而导出一种右乘形式的雅可比矩阵；当将其移动至左侧时，则会形成左乘的形式。
基于李代数的导数法则的基础上进行推导并计算得出，在机器人学中对变换矩阵进行精度评估时
以及关于 Tj 的：
考虑到 se(3) 上的左右雅可比 Jr 表达式具有较高的计算复杂度，在实际应用中通常会采用其近似形式。 当系统的误差趋近于零时，我们可以将这些雅可比矩阵近似地等于单位矩阵 I。
从理论上讲，在经过优化后的情况中,由于每条边提供的观测数据不一致,并且误差通常也不见得趋近于零
掌握雅可比矩阵的概念后,后续的过程与常规的图优化方法并无本质区别。换句话说,所有的位姿顶点以及连接这些顶点的约束关系构成了一个典型的图优化问题。核心在于这是一个最小二乘问题,其优化变量是各个顶点的位姿参数,而这些约束关系来源于对各个顶点位置的观测结果。
记 E 为所有边的集合，那么总体目标函数为：
此问题同样可基于Gauss-Newton方法及其变体进行求解。
除了以李代数为手段表示外,其他优化过程均呈现相似特征。
此问题自然可采用Ceres框架或g2o库进行实现。
参考：
相关推荐：
视觉SLAM笔记（54） Ceres 实现后端优化
视觉SLAM笔记（53） g2o 实现后端优化
视觉SLAM笔记（52） BA 与图的结合应用
此外，在非线性系统中与EKF结合应用方面也有深入研究 视觉SLAM笔记（51） 非线性系统和 EKF
同时在线性系统中与KF结合应用也有重要探讨 视觉SLAM笔记（50） 线性系统和 KF
谢谢！