我的学习笔记

Global Optimization 一、全局优化的目标与核心思想 在视觉惯性SLAM (Simultaneous Localization and Mapping) 系统中，全局优化 (Global Bundle Adjustment, GBA) 是一个至关重要的后端优化步骤。其核心目标是优化系统中所有关键帧的位姿（位置和姿态）以及所有三维地图点（Map Points）的空间坐标。通过这一过程，可以显著减少由于传感器噪声、运动估计误差和匹配不确定性等因素造成的累积漂移，从而构建一个全局一致且高精度的地图，并提升相机自身的定位精度。 核心思想：捆绑调整 (Bundle Adjustment, BA) 全局优化的数学实质是执行一次大规模的捆绑调整。这个名字形象地描述了其工作原理：想象场景中的三维点通过光线（bundles of rays）投影到不同时刻相机捕获的图像上。BA的目标就是通过微调每一个关键帧的位姿和每一个三维地图点的位置，使得这些三维点在各个观测到它的关键帧图像上的重投影误差 (Reprojection Error)...

本章节主要阐述在ORB-SLAM2系统中，当检测到闭环（Loop Closing）后，如何通过优化一个称为“本质图”（Essential Graph）的结构来全局校正和优化所有关键帧的位姿。这个过程对消除累积误差、提升地图全局一致性和定位精度至关重要。 一、优化的核心目标与对象 核心函数：OptimizeEssentialGraph。 主要目标：在闭环确认后，对系统中所有已记录的关键帧的位姿（包括位置和姿态，即Sim(3)变换）进行一次全局性的优化调整。 关键点： 不直接优化地图点：在这一特定步骤中，优化过程的变量是关键帧的位姿，而不是三维地图点的坐标。地图点的位置会在关键帧位姿优化完毕后，根据其参考关键帧的位姿更新进行调整。 全局性：优化会影响到地图中所有的关键帧，以达到全局最优的目的。 二、本质图的构成：顶点...

闭环线程中的 Sim(3) 位姿优化 理解在检测到潜在的闭环时，系统如何优化当前关键帧 (KF1) 与闭环候选关键帧 (KF2) 之间的相对位姿。此相对位姿通过一个 Sim(3) 变换（包含旋转、平移和尺度信息）来描述。关键点在于，此优化过程仅针对 Sim(3) 位姿进行，地图点的三维坐标在此步骤中保持固定，不参与优化。 一、核心思想：图优化 (Graph Optimization) 该优化过程依赖于图优化方法，具体实现通常借助 g2o (General Graph Optimization) 框架。图优化的基本组成元素如下： 顶点 (Vertices)： 图中的节点，代表了系统中待优化的变量。在我们的场景中，主要是 Sim(3) 位姿。 边 (Edges)： 连接顶点的边，代表了这些变量之间存在的约束关系。这些约束通常以误差函数的形式表达，优化的目标是最小化所有误差的总和。 二、顶点的选择 (Vertices) 在闭环的 Sim(3) 位姿优化问题中，主要涉及以下两种类型的顶点： 待优化的 Sim(3) 位姿: 描述：...

局部建图线程中局部地图优化 (Local Bundle Adjustment in Local Mapping Thread) 引言 在 ORB-SLAM2 系统中，局部建图线程负责处理新的关键帧，并优化局部的地图。LocalBundleAdjustment (局部BA) 是该线程中的核心函数，主要用于优化当前关键帧及其邻近区域的关键帧位姿和它们观测到的地图点三维位置，以构建一个局部一致的地图。 核心思想 全局的 Bundle Adjustment (BA) 会优化所有的关键帧和地图点，计算成本非常高。局部BA的思想是只对当前活跃的、与当前关键帧紧密相关的一个子图进行优化。这大幅降低了计算复杂度，使得系统能够实时运行，同时通过不断优化局部区域，逐步累积实现全局地图的一致性。 关键概念 (参考图 14-2) 理解局部BA，首先要明确参与优化的不同元素： 当前关键帧 (Current KF): 定义：最新插入到地图的关键帧，是本次局部BA的触发者和中心。 图14-2标识：中间的红色相机图标 (“当前KF”)。 局部关键帧 (LocalKeyFrames /...

1. 概述 PoseOptimization 函数是ORB-SLAM2中用于跟踪线程 (Tracking Thread) 的一个关键优化步骤。其核心目标是： 仅优化当前帧的相机位姿 (SE(3)SE(3)SE(3)变换)。 不优化地图点 (MapPoints) 的三维位置，即假设地图点在世界坐标系中的位置是固定的。 此函数主要应用在跟踪过程中的运动跟踪、参考帧跟踪、地图跟踪和重定位阶段。 2. 图优化基础：顶点与边 该优化过程基于图优化理论，使用g2o库来实现。图由顶点和边构成。 2.1. 顶点 (Vertex) 定义: 代表当前帧的相机位姿，这也是本次优化的待优化变量。 表示: 通常是一个 SE(3)SE(3)SE(3) 变换，描述了从世界坐标系 (World Frame) 到当前相机坐标系 (Camera Frame) 的旋转 (RRR) 和平移 (ttt)。 Tcw=(Rcwtcw01)T_{cw} = \begin{pmatrix} R_{cw} & t_{cw} \\ 0 & 1...

Local Map Tracking 1. 目标与定位 角色：第二阶段跟踪，在初步跟踪（参考关键帧、恒速模型、重定位）之后进行。 目的：利用更丰富的局部地图信息，对第一阶段获得的当前帧位姿进行精细化优化，得到更准确的位姿。 核心：增加匹配约束，仅优化当前帧位姿。 2. 与“局部建图线程 (Local Mapping)”的区别 局部地图 (Local Map) - 用于本阶段（局部地图跟踪） 数据：由局部关键帧及其观测到的局部地图点构成。 功能：为当前帧提供投影匹配候选，仅优化当前帧位姿。不改变关键帧位姿，也不改变地图点三维坐标。 本质：“用图”来精调当前帧。 局部建图线程 (Local Mapping) - 独立线程 数据：处理新生成的关键帧。 功能： 生成新的地图点。 进行局部BA (Bundle Adjustment)，同时优化局部区域内的关键帧位姿和地图点坐标。 剔除冗余关键帧和不可靠地图点。 本质：“建图”与“维护图”。 3. 局部地图跟踪的核心步骤 第 1...

Relocalization 一、核心目标与触发时机 核心目标： 当系统由于各种原因（如快速运动、场景剧变、遮挡等）导致跟踪丢失，即无法确定当前相机帧相对于已有地图的位姿时，重定位模块的目标是在已构建的地图中重新找回当前帧的准确位姿。 触发时机： 这是 SLAM 系统中的一种**“拯救”或“恢复”机制**。它在常规的、依赖连续性的跟踪方法（如恒速模型跟踪、参考关键帧跟踪）均宣告失败后被激活。 二、核心思想：全局搜索与局部验证 重定位的整个过程可以概括为两个主要阶段，体现了从粗略的全局匹配到精确的局部验证的思想： 全局外观搜索 (Global Appearance-Based Search)： 由于跟踪已经丢失，系统对当前帧的位姿没有任何先验信息或良好的初始估计。 策略： 利用词袋模型 (Bag-of-Words, BoW) 在整个历史关键帧数据库 (KeyFrame Database) 中进行快速、大规模的搜索。 目的： 找出那些在视觉外观上与当前丢失帧最相似的候选关键帧 (Candidate...

TrackWithMotionModel 一、核心思想与定义 恒速模型跟踪基于一个核心假设：在连续的图像帧之间极短的时间内（通常为几十毫秒），相机的运动状态可以近似为匀速运动。 因此，可以利用上一帧的位姿以及先前估计得到的**帧间相对运动（速度和角速度，统称为速度 V）来预测当前帧的位姿，并以此为初始值进行后续的匹配和优化。 二、为何使用恒速模型跟踪？ 效率优先： 在地图初始化并通过参考关键帧跟踪成功获得初始速度后，恒速模型提供了一种更简单、计算更快的位姿估计方法。 相比于参考关键帧跟踪（涉及词袋匹配等），恒速模型直接利用运动先验，对实时性要求高的SLAM系统非常重要。 常用策略： 一旦系统有了速度信息，恒速模型便成为跟踪线程中的首选和最常用的跟踪方法。 三、关键公式 速度 (Velocity) V\mathbf{V}V： 表示从上一帧 (l) 到当前帧 (c) 的相对位姿变换 Tcl\mathbf{T}_{\mathrm{cl}}Tcl​。 如果上一帧的世界位姿为 Tlw\mathbf{T}{\mathrm{lw}}Tlw，当前帧的世界位姿为...

TrackReferenceKeyFrame 一、核心目标 当相机捕获到新的普通帧（位姿未知）时，利用一个已知的、位姿相对准确的参考关键帧 (Reference KeyFrame) 来估计当前普通帧的精确位姿。 二、应用场景 参考关键帧跟踪主要在以下两种情况下被调用： 系统刚初始化后： 场景描述： SLAM 系统通过初始几帧（如第1、2帧）完成地图初始化，并将它们设为关键帧。当后续帧（如第3帧）进入时，系统尚无稳定的运动模型（例如，恒速模型中的速度为空）。 为何使用： 此时，最可靠的方式是直接将新帧与已初始化的关键帧进行匹配，以获得新帧的初始位姿。参考关键帧的位姿是后续建图和跟踪的基础，相对可靠。 恒速模型跟踪失败后： 场景描述： 系统尝试使用恒速模型预测当前帧位姿失败（可能由于相机运动剧烈、场景纹理缺乏等）。 为何使用： 恒速模型局限性： 仅依赖前一帧信息，信息量有限，速度估计可能不准，易导致错误匹配。 参考关键帧优势： 通常经过局部建图线程优化，位姿更精确；可能关联了更多地图点，3D信息更丰富。因此，作为一种更稳健的备选方案。 ...

1. 函数背景与目标 这个特定版本的 SearchByProjection 函数在 ORB-SLAM2 的闭环检测 (Loop Closing) 流程中被调用。 调用时机：当系统检测到一个潜在的闭环（当前关键帧 pKF 与过去的某个关键帧 pLoopKF 在同一位置），并且已经成功计算出连接这两个关键帧位姿的 Sim3 变换矩阵 Scw 之后。 函数目标：利用已知的 Scw 变换，为当前关键帧 pKF 中那些尚未建立地图点关联的特征点，去寻找并建立与闭环候选帧 pLoopKF 及其共视帧相关的地图点 (vpPoints) 之间的新匹配关系。其核心目的是增加闭环约束的数量和质量。 2. 核心思想 利用已计算出的 Sim3 变换 Scw 作为几何先验，指导特征匹配过程，从而在两个看似分离的地图部分之间建立更多可靠的连接点。 具体来说，是将 pLoopKF 相关区域的 3D 地图点 (vpPoints)，通过 Scw 变换投影到当前关键帧 pKF 的图像平面上，然后在投影点附近搜索匹配的 pKF 特征点。 3. 主要执行步骤与思想解析 a. 输入与准备 (Code...