1、课程导入：SLAM问题概述、因子图与非线性优化的关系、课程目标与学习路径

SLAM（Simultaneous Localization and Mapping，同步定位与建图）是移动机器人与自动驾驶领域的核心基础技术。它解决的核心问题是：一个搭载传感器的移动平台，在未知环境中如何同时估计自身位姿并构建环境地图。传统SLAM系统通常被拆解为前端（Front-end）与后端（Back-end）两个模块：前端负责数据关联与帧间位姿估计，后端则负责全局状态优化与误差消除。本课程聚焦于后端优化的数学本质——因子图（Factor Graph）与非线性优化（Nonlinear Optimization），这是现代SLAM系统（如ORB-SLAM3、LIO-SAM、VINS-Fusion）的公共基石。理解这一层，意味着你不再只是“调参工程师”，而是能够设计、改进甚至重构SLAM系统的核心算法工程师。

1.1 SLAM问题概述：从概率估计到图模型

核心原理：SLAM的数学本质

SLAM问题的标准数学描述是：给定传感器观测序列 \( z_{1:T} \) 和控制输入 \( u_{1:T} \)，估计机器人位姿 \( x_{1:T} \) 与环境路标 \( m \) 的联合后验概率 \( P(x_{1:T}, m | z_{1:T}, u_{1:T}) \)。直接求解这个高维联合分布极其困难，因此工程上通常将其转化为最大后验估计（MAP）问题，即寻找使后验概率最大的状态变量取值。由于传感器噪声通常建模为高斯分布，MAP问题进一步转化为最小化负对数后验——一个非线性最小二乘问题。这正是因子图与非线性优化结合的切入点。

因子图的引入动机

因子图是一种二分图模型，由变量节点（Variable Nodes）和因子节点（Factor Nodes）组成。变量节点表示待估计的状态量（如位姿、路标位置），因子节点表示变量之间的约束关系（如运动模型、观测模型、回环约束）。相比传统的协方差矩阵表示，因子图具有三个显著优势：稀疏性自然表达——每个因子只连接少量变量，对应雅可比矩阵的稀疏结构；增量式更新友好——新增观测只需在图中添加因子，无需重建全局结构；模块化设计——不同传感器（IMU、相机、LiDAR）的约束可以独立建模为不同类型的因子，便于多传感器融合。

案例拆解：单目视觉SLAM中的因子图构建

以ORB-SLAM3为例，其因子图包含三类核心因子：IMU预积分因子连接相邻两帧位姿，约束相对运动；视觉重投影因子连接关键帧位姿与地图点，约束观测关系；回环因子连接当前帧与历史帧，消除累积漂移。当系统检测到回环时，新增一个回环因子，触发全局Bundle Adjustment（BA）优化。整个过程体现了因子图“增量构建、批量优化”的核心思想。实际工程中，因子图的规模可达数万个节点、数十万个因子，但得益于稀疏结构，现代求解器（如GTSAM、g2o）仍能实时运行。

1.2 因子图与非线性优化的关系：从图到最小二乘

核心原理：因子图如何转化为优化问题

因子图本身只是概率模型的图形化表示，真正驱动SLAM后端运行的是其对应的非线性优化问题。每个因子 \( f_i(\theta_i) \) 对应一个残差项 \( e_i(\theta_i) \)，其中 \( \theta_i \) 是因子连接的变量集合。假设噪声服从高斯分布，因子可写为：

f_i(θ_i) ∝ exp(-0.5 * ‖e_i(θ_i)‖²_Σi)

其中 \( Σ_i \) 是信息矩阵（噪声协方差的逆）。整个因子图的联合概率为所有因子的乘积，取负对数后得到优化目标：

min Σ_i ‖e_i(θ_i)‖²_Σi

这是一个典型的非线性最小二乘问题。求解该问题的核心步骤包括：线性化（在当前位置处对残差做一阶泰勒展开）、构建正规方程（\( J^TΣ^{-1}J Δx = -J^TΣ^{-1}e \)）、求解稀疏线性系统（利用Cholesky分解或QR分解）。

判断方法：何时选择因子图优化而非滤波

在实际SLAM系统设计中，需要根据应用场景选择后端方案。以下情况优先选择因子图优化：存在回环检测需求（如室内机器人、自动驾驶）；多传感器融合（IMU+视觉+LiDAR）；计算资源充足（有GPU或高性能CPU）；需要全局一致性（如建图精度要求高）。反之，若计算资源极度受限（如微型无人机）、传感器噪声模型简单、无回环需求，传统EKF滤波仍可胜任。

案例拆解：视觉惯性导航系统（VINS）中的因子图设计

以VINS-Fusion为例，其因子图包含：IMU预积分因子（约束相邻帧的相对位姿与速度）、视觉特征因子（约束关键帧与路标点）、回环因子（约束当前帧与历史帧）、边缘化因子（保留旧状态的信息）。在优化过程中，系统采用滑动窗口策略，仅优化窗口内的关键帧，窗口外的状态通过边缘化（Marginalization）转化为先验因子。这种设计在保证精度的同时控制了计算量。实际运行中，VINS-Fusion在嵌入式平台（如Jetson TX2）上可达到30Hz的优化频率，充分体现了因子图+稀疏优化的工程优势。

1.3 课程目标与学习路径：从理论到工程落地

核心原理：本课程的知识体系架构

本课程共30章，按“数学基础 → 因子图建模 → 优化求解 → 工程实践 → 前沿扩展”五层递进设计。第一层（第1-5章）夯实数学基础，包括李群李代数、概率图模型、稀疏线性代数；第二层（第6-12章）深入因子图建模，涵盖视觉因子、IMU因子、回环因子、边缘化技术；第三层（第13-20章）讲解非线性优化算法，从高斯牛顿到LM算法，再到增量优化（iSAM2）；第四层（第21-26章）聚焦工程实践，包括g2o/GTSAM/Ceres的使用、调试技巧、性能调优；第五层（第27-30章）探讨多传感器融合、语义SLAM、学习型SLAM等前沿方向。

学习路径与能力目标

完成本课程后，学习者应达到以下能力：理论层面——能够从概率角度推导SLAM后端的MAP问题，理解因子图与非线性优化的数学等价性；算法层面——能够手写高斯牛顿求解器，理解稀疏矩阵求导与Schur消元的原理；工程层面——能够基于GTSAM或g2o搭建完整的SLAM后端，处理实际传感器数据；创新层面——能够针对特定场景设计新的因子类型或优化策略。

案例拆解：从零搭建一个因子图SLAM后端

以一个简化的2D激光SLAM为例，展示因子图后端的完整流程。假设机器人搭载2D激光雷达，在平面环境中运动。第一步，定义变量节点：机器人位姿 \( x_i = (x, y, \theta) \)，路标点 \( l_j = (x_j, y_j) \)。第二步，定义因子节点：里程计因子连接相邻位姿，约束相对运动；激光观测因子连接位姿与路标，约束距离与角度。第三步，构建优化问题：将所有残差平方和作为目标函数，使用高斯牛顿法迭代求解。第四步，加入回环因子：当机器人回到已知区域时，添加回环约束，重新优化。通过这个案例，可以直观理解因子图从建模到求解的完整链路。

常见误区与总结复盘

在学习本课程时，常见误区包括：误区一——将因子图优化等同于简单的梯度下降，忽略了稀疏结构利用的重要性；误区二——忽视李群李代数在优化中的关键作用，导致位姿更新方式错误；误区三——不理解边缘化的数学含义，在滑动窗口优化中错误地丢弃历史信息；误区四——过度依赖现成库，缺乏对底层原理的掌握，遇到问题无法定位。本课程将通过大量推导、代码实现和实验对比，帮助学习者避开这些陷阱，建立扎实的SLAM后端知识体系。