【ORB-SLAM2：九、BA优化】

BA（Bundle Adjustment）是SLAM系统中优化位姿和地图点位置的重要技术。通过最小化图结构中的重投影误差，BA在提高地图精度和轨迹优化方面发挥了核心作用。本章将围绕BA优化展开，从图优化工具简介到优化函数分类，再到具体的局部BA和Sim3优化边的解析进行详细阐述。

---

9.1 图优化和g2o简介

9.1.1 图优化的基本概念

1. 图优化
   图优化将SLAM问题建模为一个图结构：

   • 节点（Vertices）：代表关键帧的位姿或地图点的三维坐标。
   • 边（Edges）：代表节点间的约束，通常由传感器测量或视觉匹配获得。
   • 目标：通过最小化边上的误差函数，使图中的节点状态尽可能符合观测值。

2. 优化方法
   图优化通常采用非线性优化方法，例如：

   • 高斯牛顿法。
   • Levenberg-Marquardt方法。
   • Dogleg方法。

9.1.2 g2o简介

1. g2o简介
   g2o（General Graph Optimization）是一个广泛用于SLAM领域的开源图优化库，具有以下特点：

   • 支持稀疏矩阵计算，加速大规模优化问题的求解。
   • 提供可扩展的优化框架，支持自定义误差函数和数据类型。
   • 应用灵活，可处理多种优化问题，如BA、Pose Graph等。

2. g2o的核心模块

   • Vertices（顶点）：定义优化变量，例如相机位姿、三维点。
   • Edges（边）：定义顶点间的约束，例如重投影误差。
   • Optimizers（优化器）：实现具体的优化过程。

3. g2o在ORB-SLAM中的应用

   • 用于全局和局部BA优化。
   • 支持Sim(3)优化以处理尺度变化问题。

---

9.2 优化函数分类

9.2.1 BA优化中的误差函数

1. 重投影误差
   重投影误差是BA优化中最常用的误差函数，定义为：

   $$e=\sum _i\Vert {\mathbf{z}}_i-\pi ({\mathbf{T}}_i{\mathbf{X}}_i){\Vert }^2$$

   • ${\mathbf{z}}_i$ ：图像平面上的观测点。
   • $\pi$ ：投影函数，将三维点投影到图像平面。

2. Sim(3)误差
   用于处理相机位姿的尺度误差，误差函数形式为：

   $$e=\Vert {\mathbf{T}}_i^{-1}{\mathbf{T}}_j{\mathbf{X}}_j-{\mathbf{X}}_i{\Vert }^2$$

   • 包括旋转、平移和尺度因子。

9.2.2 优化目标分类

1. 全局优化

   • 对整个地图中的关键帧和地图点进行优化。
   • 优点：精度高。
   • 缺点：计算量大，实时性较差。

2. 局部优化

   • 仅优化当前帧和其邻域内的关键帧及地图点。
   • 优点：实时性强。
   • 缺点：全局一致性可能较弱。

---

9.3 局部BA中边的解析

局部BA是SLAM系统中提高实时性的重要优化步骤，其目标是通过对局部区域进行优化，减少计算复杂度，同时保证局部精度。

9.3.1 局部BA的基本框架

1. 优化范围
   局部BA通常包括以下元素：

   • 当前帧及其直接相邻的关键帧。
   • 邻域关键帧中观测到的所有地图点。
   • 这些地图点的相关观测帧。

2. 优化流程

   • 构建局部图：提取优化范围内的节点和边。
   • 定义误差函数：使用重投影误差。
   • 进行迭代优化：调整关键帧位姿和地图点位置。

9.3.2 边的解析

1. 边的定义
   局部BA中，边表示关键帧和地图点之间的重投影误差：

   • 一个边连接一个关键帧的位姿和一个地图点的三维位置。
   • 边的权重由观测质量决定。

2. 边的约束类型

   • 视觉约束：通过特征点匹配构建。
   • 几何约束：通过三角化过程获得。
   • 惯性约束：如果使用IMU，可增加额外的边。

9.3.3 局部BA的实现细节

1. 优化变量

   • 顶点类型：相机位姿和地图点。
   • 边的类型：重投影误差边。

2. 边的选择策略

   • 优先选择高质量的边。
   • 剔除具有明显误差的边以避免优化发散。

---

9.4 Sim3优化中边的解析

Sim3优化用于处理具有尺度变化的位姿关系，特别是在闭环矫正或地图合并时。

9.4.1 Sim3优化的基本概念

1. 相似变换
   Sim3包含旋转、平移和尺度因子，用于描述两个坐标系间的关系。

2. 优化目标

   • 优化Sim3变换参数，使两组关键帧间的误差最小化。
   • 同时校正关键帧的尺度差异。

9.4.2 Sim3边的定义

1. 边的构成

   • Sim3边连接两个关键帧的位姿。
   • 边的误差由两关键帧间的观测点对给出。

2. 误差函数

   $$e=\sum _i\Vert {\mathbf{X}}_i^{\text{current}}-\text{Sim3}({\mathbf{X}}_i^{\text{reference}}){\Vert }^2$$

   • ${\mathbf{X}}_i^{\text{current}}$ ：当前帧中点的坐标。
   • ${\mathbf{X}}_i^{\text{reference}}$ ：参考帧中点的坐标。

9.4.3 边的优化方法

1. 初始估计
   • 使用匹配点对计算初始的Sim3变换。
2. 误差最小化
   • 在优化过程中，Sim3的旋转和平移以李代数形式表示。
   • 使用高斯牛顿或Levenberg-Marquardt方法进行优化。

9.4.4 实现细节

1. 边权重的设置

   • 边的权重取决于观测点对的质量和数量。
   • 剔除不符合几何约束的边。

2. 实时性保障

   • 优化过程中仅保留最相关的关键帧对以减少计算复杂度。

通过本章内容的详细分析，BA优化在SLAM系统中占据关键地位。从图优化工具g2o的使用，到优化函数分类，以及局部BA和Sim3优化边的解析，我们全面理解了BA优化的理论和实现细节。这为SLAM系统的精度和鲁棒性奠定了坚实基础。