SLAM基础知识速记

• 1.ORB特征提取与匹配
• 2.帧间位姿估计
  • 2.1 2D-2D
  • 2.2 2D-3D
  • 2.3 3D-3D
• 3.RANSAC方法
• 4.卡尔曼滤波
• 5.IMU
• 6.高斯-牛顿与列文-马夸法比较
  • 6.1 LM方法
  • 6.2 GN方法
• 7.双视图BA
• 8.滤波方法与图优化方法对比
• 9.信息矩阵
• 10.ORB-SLAM中的优化
  • 10.1 只优化位姿(Motion-Only BA)
  • 10.2 同时优化地图点和位姿(Local BA)
  • 10.3 视觉-IMU联合优化
• 11.直接法与特征点法对比
• 12.SLAM中的回环
  • 12.1 回环检测
  • 12.2 回环校正
• 13.张正友标定法
• 14.ORB-SLAM中IMU的初始化
• 15.IMU预积分
• 16.回环检测和系统重定位异同
• 17.回环本质图
• 18.数组(Array)
• 19.链表(Linked List)
• 20.栈(Stack)
• 21.队列(Queue)
• 22.双端队列(Deque)
• 23.哈希表(Unordered_map)
• 24.映射(Map)
• 25.集合(Set)
• 26.动态数组(Vector)
• 27.C++面向对象开发特性
• 28.嵌入式平台SLAM优化方向
• 29.SLAM线程
• 30.代码版本控制与合作
• 31.位姿图优化
• 32.SLAM和SfM
• 33.ORB-SLAM3的特点
• 34.ROS

之前在9月份的时候，整理了几张SLAM知识相关笔记，这里简单将其再贴出来，以备之后使用。

1.ORB特征提取与匹配

• 提取：FAST角点 + BREIF描述子
• 匹配：暴力匹配

2.帧间位姿估计

2.1 2D-2D

• 原理：对极几何
• 步骤：变换矩阵求解 + 矩阵分解求位姿
• 变换矩阵类型包括：
  • 本质矩阵 - 非平面场景、内参已知
  • 基础矩阵 - 非平面场景、内参未知
  • 单应矩阵 - 平面场景、内参未知
• 矩阵分解核心：SVD分解，利用SVD分解求最小二乘解，得到\(\mathbf{U}\)、\(\mathbf{\Sigma}\)、\(\mathbf{V}\)。

2.2 2D-3D

• 本质：PnP问题(Perspective-n-Point)
• 解法：直接线性变换DLT、P3P、EPnP等
  • DLT：简单、速度快、精度低，要求≥6对点
  • P3P：最低3对点即可，但易受噪声影响
  • EPnP：线性复杂度、精度可迭代，要求≥4对点
  • 一般默认都采用EPnP解法

2.3 3D-3D

• 核心是构建对应方程，常见有SVD法和ICP法
• SVD法：基于方程直接SVD分解，然后根据\(\mathbf{U}\)、\(\mathbf{\Sigma}\)、\(\mathbf{V}\)，按公式计算旋转、平移。SVD分解中协方差矩阵的物理意义表示两组XYZ系统互相关系，大小为3×3。SVD法具体步骤如下：
  • (1) 计算质心
  • (2) 构建协方差矩阵
  • (3) 去中心化
  • (4) SVD分解
  • (5) 计算旋转矩阵
  • (6) 计算平移向量
• ICP法：迭代最近邻法(Iterative Closest Point)。匹配源点云和目标点云对应点，最小化对应点间的距离残差。优点：简单高效；缺点：对初值敏感。ICP法步骤如下：
  • (1) 初始对齐/配准
  • (2) 迭代优化
    • 找最近点
    • 剔除异常
    • 求解变换
    • 更新点云
  • (3) 迭代终止
• SVD方法：需要点云对应关系、不需要预配准
• ICP方法：无需点云对应关系、需要预配准

3.RANSAC方法

• 背景：虽然最小二乘很好，但其对外点敏感
• 核心：找到一个适合于绝大多数样本的模型参数。其本身并非进化算法，并不会随着迭代次数的增加而精度一定更高，而是随机计算指定次数，从里面找到最好的。只能说随机次数越多，找到精度越高模型的概率越大。
• 步骤：
  • (1) 随机选择指定数量点构建模型
  • (2) 利用该模型计算整体内点个数
  • (3) 选取内点最多对应的模型

4.卡尔曼滤波

• 核心为状态预测与测量更新两个步骤。
• 状态预测：
  • (1) 根据上一个状态利用模型，预测当前状态
  • (2) 根据给定误差与上一个状态误差，预测当前状态误差
• 测量更新：
  • (3) 根据当前预测状态、实际观测和观测矩阵，计算卡尔曼增益
  • (4) 根据当前预测状态、实际观测和观测矩阵，结合卡尔曼增益平衡观测与预测权重，修正当前状态预测
  • (5) 根据卡尔曼增益、观测矩阵和上一个状态预测误差，修正更新后状态预测对应的误差

5.IMU

• 误差：常量偏置、偏置稳定性、随机游走机械白噪声、温度误差
• 姿态推理：陀螺仪
• 位置推理：陀螺仪+加速度计
• 误差消除/校正：
  • 常量偏置：离线静置消除
  • 偏置稳定性：滤波方法
  • 白噪声：随机游走建模

6.高斯-牛顿与列文-马夸法比较

• 两者都用于非线性最小二乘求解
• 区别在于增量方程的构建
• GN：残差小、初值好、实时高
• LM：残差大、初值差、尺度不一

一般使用LM方法更多。

6.1 LM方法

• 线性化，构建\(\mathbf{J}^{T}r\)与\(\mathbf{J}^{T}\mathbf{J}\)
• 算阻尼，解\((\mathbf{J}^{T}\mathbf{J}+\mu l)\Delta x = - \mathbf{J}^{T}r\)
• 判进退，误差下降接受\(\mu / 3\)，否则\(2\mu\)
• 循环直到\(\left \| \Delta x \right \| < \varepsilon\)

6.2 GN方法

• 计算残差\(r\)和雅可比矩阵\(\mathbf{J}\)
• 解线性方程\((\mathbf{J}^{T}\mathbf{J})\Delta x=-\mathbf{J}^{T}r\)
• 更新\(x\leftarrow x+\Delta x\)，直到\(\left \| \Delta x \right \| < \varepsilon\)

GN方法中，\(\mathbf{J}^{T}\mathbf{J}\)近似为海塞矩阵。其总体表示误差曲面的形状张量，对角线表示沿i参数的自弯曲，非对角元素表示ij参数的耦合弯曲。

7.双视图BA

• 核心：同时优化位置与姿态，最小化重投影误差
• 节点：相机位姿、特征点空间位置
• 边：投影关系
• 线性化 - 正规方程 - 迭代更新

8.滤波方法与图优化方法对比

9.信息矩阵

• 表示对不确定度按方向的打分，等于协方差矩阵的逆(不确定度的倒数)。
• 信息矩阵特征值越大则方向越确定，信息矩阵决定残差边权重。
• 非对角块：位姿&路标间的信息耦合程度
• 对角块：总观测确定度

10.ORB-SLAM中的优化

基于g2o库构建

10.1 只优化位姿(Motion-Only BA)

• 节点：当前位姿(no fixed)
• 边：地图点观测(fixed)

10.2 同时优化地图点和位姿(Local BA)

• 节点：直接相邻KeyFrame、间接相邻KeyFrame、地图点
• 边：单/双目边观测

10.3 视觉-IMU联合优化

• 核心为：共视关系、时序关系
• 节点：时序关键帧位姿、时序关键帧IMU、局部视觉关键帧位姿、固定关键帧位姿、固定关键帧IMU、地图点
• 边：单目边、双目边、惯性边、陀螺仪偏置边、加速度计偏置边
• 与纯视觉BA相比，增加了时序相邻关键帧的IMU观测。
• 共视用于视觉优化、时序用于惯性优化，两者共同作用于当前位姿节点，使其优化。

11.直接法与特征点法对比

12.SLAM中的回环

SLAM中包含回环检测和回环校正两方面。

12.1 回环检测

主要利用BoW词袋法，实现逐步筛选，包含两个步骤：

• 共视帧BoW相似性计算
• 共视一致性检查(几何检查)

几何检查包括：共视一致性、尺度一致性、旋转/平移合理性、双向投影误差

12.2 回环校正

一次性消除累积误差，步骤：计算sim3闭环边 - 传播位姿 - 融合地图点 - 本质图对齐 - 后台全局BA

• 利用RANSAC-PnP计算sim3作为闭环约束边(相似变换)
• 将sim3传播给回环帧的所有共视帧并更新位置
• 地图点融合与替换
• 更新共视图与生成树，只优化闭环相关因子图
• 启动全局BA

13.张正友标定法

• 棋盘格准备并拍摄
• 检测角点
• 构造世界坐标
• 对每张图求解单应矩阵(3D-2D)
• 构造约束，解线性内参，Cholesky分解
• 非线性Refine，最小化投影误差
• LiDAR-FAST-Calib、IMU-Kalibr

14.ORB-SLAM中IMU的初始化

• 首先基于纯视觉优化位姿
• 然后进行纯IMU优化
• 最后进行视觉-IMU联合优化
• 参数：重力方向、尺度、陀螺仪偏置、加速度计偏置、速度

15.IMU预积分

把高频IMU观测压缩为关键帧间的一个紧凑因子，避免逐帧误差累积、重复利用。

包含：相对旋转、速度、位置、协方差雅可比矩阵

16.回环检测和系统重定位异同

重定位用于在LOST后继续Tracking，而回环检测是为了构成回环约束条件。前者更松、实现不同。前者在Tracking中，后者在LoopClosing中。

17.回环本质图

压缩版共视图，只保留生成树边+回环边+强共视边，节点数不变。边的计算复杂度从\(O(n^2)\)到\(O(n)\)。

• 生成树边：父子关系，保证联通
• 回环边：Sim3验证后添加
• 强共视边：阈值判别

18.数组(Array)

直接访问元素，时间复杂度为O(1)

• 优点：访问速度快，内存紧凑
• 缺点：无法动态扩展，不适合动态数据

19.链表(Linked List)

动态调整大小，线性查找，复杂度为O(n)

• 优点：动态查找，适合频繁的插入/删除
• 缺点：随机访问效率低，不如数组

20.栈(Stack)

后进先出LIFO，常用于递归，深度优先搜索

• 优点：操作简单，效率高
• 缺点：只能在栈顶操作，访问其它元素需弹出(无法随机访问)

21.队列(Queue)

先进先出FIFO，常用于广度优先搜索、任务调度等任务

• 优点：适合按顺序处理场景
• 缺点：无法随机访问元素

22.双端队列(Deque)

允许两端进行插入、删除，是栈和队列的结合体

• 优点：灵活双向操作
• 缺点：空间占用较大

23.哈希表(Unordered_map)

键值对存储数据，无序，查找特定元素时间复杂度为O(1)

24.映射(Map)

有序键值对，底层是红黑树，查找、插入、删除复杂度为O(logn)

• 优点：元素有序
• 缺点：效率比unordered_map低

25.集合(Set)

存储唯一元素的有序集合，底层是红黑树

• 优点：自动排序且保证唯一
• 缺点：插入、删除不如无序集合

26.动态数组(Vector)

• 优点：动态容量，支持随机访问
• 缺点：插入、删除中间元素效率低

27.C++面向对象开发特性

封装、继承、多态、抽象

• 封装：将数据和方法组合在一起，对外隐藏细节，对外提供接口
• 继承：从已有类中派生出新类，可修改扩展属性和方法
• 多态：同一种操作用于不同对象，可以有不同实现
• 抽象：从具体实例中提取特征形成抽象接口，以便扩展

28.嵌入式平台SLAM优化方向

• 算法：轻量化
• 模型：压缩
• 硬件：协同设计
• 系统：部署与调度

29.SLAM线程

包含Tracking(主)、LocalMapping、LoopClosing、Optimizer。

30.代码版本控制与合作

• 使用Git
• 分支管理：主分支、开发分支、功能分支、修复分支
• 代码审查
• 形成文档和知识共享
• 统一代码风格、备注
• 权限管理

31.位姿图优化

位姿图只包含位姿，用相对位姿边表达里程计和闭环，把误差修正问题转成稀疏图上的位姿图优化。

32.SLAM和SfM

• SLAM：实时、在线、有序、定位
• SfM：非实时、离线、无序、建图

33.ORB-SLAM3的特点

高精度、鲁棒、多传感器、多地图

• ORB-SLAM在精度、回环、多地图、兼容性上有优势
• VINS在高动态、弱纹理等有优势

长期一致性SLAM更优，短期VIO更优。

34.ROS

ROS中有Topic、Service、Action等类型。