专栏系列2D/3D/视觉SLAM理论详解共10篇 |难度高级 |预计阅读28分钟前置知识贝叶斯滤波第1章、非线性优化第2章、图优化理论第5章本文定位第6篇对比三种SLAM后端方法——从2D激光的RBPF到视觉的滑窗优化摘要本章从理论层面比较三种SLAM后端的两大阵营——滤波方法和优化方法——在2D激光、3D激光和视觉中的不同应用。重点揭示(1) 为什么RBPF在2D激光SLAM中有效但在视觉中不可行(2) EKF不一致性的数学根源(3) 现代3D激光SLAM中的IEKFFAST-LIO2如何通过迭代更新缓解不一致性(4) 滑窗优化和边缘化在三种SLAM中的不同应用策略。目录1. 滤波与优化两种后端的哲学差异2. 2D激光后端RBPF与GMapping3. EKF-SLAM与3D激光中的IEKF4. Visual SLAM的后端演化5. 滑窗优化的理论基础6. 三种后端策略的统一对比1. 滤波与优化两种后端的哲学差异1.1 处理历史信息的方式滤波方法Filtering维护当前时刻的状态后验通过边缘化丢弃所有历史变量每个时刻执行 predict → update → marginalize 的三步循环历史线性化误差无法修正一旦边缘化历史信息被压缩且不可逆优点状态规模恒定适合严格的在线实时估计优化方法Smoothing/Optimization维护一段窗口内的完整状态所有位姿路标在新信息到来时重新优化整个窗口可以回头修正过去的估计回环修正的本质优点精度远高于滤波代价状态规模随时间增长1.2 三种SLAM中的后端选择2D激光3D激光视觉经典滤波方法RBPFGMappingIEKFFAST-LIO2EKF-SLAM早期现代优化方法位姿图优化Cartographer因子图优化LIO-SAMBAORB-SLAM3混合/特殊CSMBBCartographer前端滑窗优化滑窗BA主流趋势滤波→优化过渡中IEKF仍占据重要地位优化完全取代滤波关键问题为什么滤波在2D/3D激光SLAM中仍然占有一席之地但在视觉SLAM中几乎被优化完全取代2. 2D激光后端RBPF与GMapping2.1 Rao-Blackwellization分解GMapping的核心是RBPFRao-Blackwellized Particle Filter。它利用了2D激光SLAM的一个关键条件独立性给定完整的机器人轨迹x 1 : t \mathbf{x}_{1:t}x1:t栅格地图的各栅格的后验估计是条件独立的。数学表述为p ( x 1 : t , m ∣ z 1 : t , u 1 : t ) p ( x 1 : t ∣ z 1 : t , u 1 : t ) ⏟ 粒子滤波估计轨迹 ⋅ ∏ c ∈ cells p ( m c ∣ x 1 : t , z 1 : t ) ⏟ 独立估计每个栅格 p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \underbrace{p(\mathbf{x}_{1:t} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})}_{\text{粒子滤波估计轨迹}} \cdot \prod_{c \in \text{cells}} \underbrace{p(\mathbf{m}_c \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t})}_{\text{独立估计每个栅格}}p(x1:t,m∣z1:t,u1:t)粒子滤波估计轨迹p(x1:t∣z1:t,u1:t)⋅c∈cells∏独立估计每个栅格p(mc∣x1:t,z1:t)FastSLAM对应的是路标的条件独立性给定轨迹后各路标独立GMapping对应的是栅格的条件独立性。2.2 为什么RBPF在2D激光中有效RBPF在2D激光SLAM中运作良好的三个条件(1) 低维状态空间轨迹是SE(2)3维。每个粒子仅需维护3维的轨迹变量——粒子滤波在低维空间中有效在高维空间中如视觉SLAM的6维数百个路标会遭遇维度灾难。(2) 高精度观测激光测距精度~1-3cm信噪比极高。高精度观测意味着似然函数p ( z t ∣ x t , m ) p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m})p(zt∣xt,m)高度集中——绝大多数粒子的权重接近零仅少数粒子匹配当前观测。这使得粒子滤波能高效地收敛。(3) 提议分布改进GMapping的关键贡献是改进的提议分布。标准RBPF从里程计运动模型采样提议分布宽导致大多数样本在似然函数下权重极低。GMapping先用扫描匹配ICP得到一个高斯近似的最优建议分布x t ∼ N ( x t ICP , Σ ICP ) \mathbf{x}_t \sim \mathcal{N}(\mathbf{x}_t^{\text{ICP}}, \mathbf{\Sigma}^{\text{ICP}})xt∼N(xtICP,ΣICP)从中采样。这使得有效样本数大幅提升——同等粒子数下覆盖的模式更多。2.3 RBPF vs 位姿图优化的权衡RBPF (GMapping)位姿图优化 (Cartographer)地图类型栅格地图每粒子独立地图子图全局位姿图回环处理隐式粒子重采样后回溯显式回环检测→位姿图优化→重新拼接子图计算量O ( M ⋅ N ) O(M \cdot N)O(M⋅N)M MM粒子数O ( ( 3 K ) 3 ) O((3K)^3)O((3K)3)K KK节点数内存高每个粒子独立地图中子图位姿图大规模适用性差粒子数不随面积增长好位姿图规模随面积线性增长全局一致性较差不能真正修正历史好回环后全局优化Cartographer最终取代GMapping成为2D激光SLAM的主流正是因为位姿图优化在处理大规模地图和回环修正上的压倒性优势。3. EKF-SLAM与3D激光中的IEKF3.1 EKF-SLAM的不一致性根源EKF-SLAM最严重的问题是不一致性Inconsistency——总是低估真实的不确定性过于自信。三个理论根源Huang Dissanayake, 2006(1) 线性化点漂移Linearization Point DriftEKF中均值每帧更新后变化但协方差矩阵中的块是在旧的线性化点处计算得到的。当均值移动后这些旧线性化点不再准确——基于它们的协方差无法正确反映新均值处的不确定性。(2) 观测模型的非线性视觉SLAM的透视投影和3D激光的点到线/面距离都是非线性函数。EKF的一阶泰勒展开在非线性强的区域产生系统性的线性化偏差这个偏差在每次更新中累积。(3) 为什么优化方法不存在这个问题图优化方法在每次迭代中在当前的线性化点重新评估所有残差和雅可比。关键是可以回到同一个变量和不同的线性化点重新做线性化——EKF一旦边缘化历史位姿就永久性地失去了重新线性化的能力。3.2 FAST-LIO2的IEKF迭代缓解不一致性FAST-LIO2使用**迭代扩展卡尔曼滤波IEKF**来缓解EKF的不一致性标准EKF在当前均值处做一次线性化和一次更新。IEKF在更新步中做多次迭代。每次迭代中在当前已更新的均值处重新计算观测模型的雅可比H k \mathbf{H}_kHk用新的H k \mathbf{H}_kHk重新计算卡尔曼增益和更新量重复直到收敛这相当于在后验均值处做线性化而非先验均值处减少了线性化误差。IEKF与优化的等价性IEKF的迭代更新等价于在当前状态处做一个高斯-牛顿迭代——它用后验均值作为新的线性化点。这在一定程度上弥合了滤波和优化之间的理论鸿沟。但IEKF仍然不如全优化因为它继承了滤波的边缘化不可逆问题——一旦某帧被边缘化它的线性化点就被永久锁定。3.3 为什么IEKF在3D激光中保持竞争力因素对IEKF有利对位姿图优化有利实时性要求严格恒定时间每次更新量固定时间随图增大而增长回环处理困难需要数据回溯自然处理计算平台嵌入式MCU可运行需要较强的CPU精度要求局部里程计精度足够全局一致性要求高FAST-LIO2的IEKF在局部里程计无回环中提供与全优化相当甚至更好的精度和速度。但当需要全局一致性和回环修正时因子图优化LIO-SAM的架构更优。4. Visual SLAM的后端演化4.1 从EKF到BA的范式转换视觉SLAM的后端经历了从滤波到优化的范式转换2000-2008EKF-SLAMDavison, 2003。单目EKF-SLAM状态向量包含相机位姿所有路标。随着路标从数十增长到数千O ( N 2 ) O(N^2)O(N2)的协方差更新变得不可行3000个路标→9000维协方差→每步更新~8 × 10 7 8 \times 10^78×107次运算。2007-2014PTAM的分叉架构 BA。PTAM证明了BA可以在普通PC上实时运行。Keyframe-based BA仅优化关键帧大幅减少了优化频率和规模。2015-至今ORB-SLAM3的全BA 位姿图优化。局部BA共视图内 全局位姿图回环后 全BA回环精化的三层优化策略。4.2 为什么视觉SLAM必须使用优化视觉SLAM不使用滤波的根本原因不是精度——而是路标的数量级2D激光RBPF粒子维护的是栅格地图固定的分辨率数组O ( M ⋅ cells ) O(M \cdot \text{cells})O(M⋅cells)不随路标数增长3D激光IEKF状态向量仅含位姿偏置少数活跃特征O ( 20 2 ) 400 O(20^2) 400O(202)400维视觉EKF-SLAM状态向量含所有路标O ( n 2 ) O(n^2)O(n2)复杂度n ≈ 5000 n \approx 5000n≈5000→ 完全不实时视觉BA的O ( m 3 n ) O(m^3 n)O(m3n)复杂度经过Schur消元使得在5000路标下仍然可实时求解。5. 滑窗优化的理论基础5.1 滑窗优化 精度与效率的折中滑窗优化是在全批图优化精度最高、计算量随窗口增大和滤波计算量恒定、精度受限之间的工程折中。维护最近k kk帧k kk通常7-15每新帧到来时优化窗口内所有变量位姿路标/偏置边缘化超出窗口的最老变量通过Schur补保留先验信息5.2 三种SLAM中的滑窗策略2D激光3D激光视觉是否使用滑窗通常不使用是是窗口大小∞全历史50-100帧7-15关键帧边缘化对象—最老帧最老帧 或 旧路标Fill-in管理—定期重置窗口边缘化路标而非帧5.3 边缘化 信息保留 Fill-in代价边缘化通过Schur补保留被移除变量的历史信息。代价是引入填充Fill-inp ( x in ) ≈ N ( x ^ in , Σ in − Σ in,old Σ old,old − 1 Σ old,in ) p(\mathbf{x}_{\text{in}}) \approx \mathcal{N}(\hat{\mathbf{x}}_{\text{in}}, \mathbf{\Sigma}_{\text{in}} - \mathbf{\Sigma}_{\text{in,old}}\mathbf{\Sigma}_{\text{old,old}}^{-1}\mathbf{\Sigma}_{\text{old,in}})p(xin)≈N(x^in,Σin−Σin,oldΣold,old−1Σold,in)三种SLAM中fill-in的可接受度差异2D激光不边缘化→无此问题每帧仅3维的全历史保留可行3D激光每帧15维50-100帧滑窗→fill-in是主要瓶颈→定期重置窗口丢弃先验视觉路标的3×3对角块使得边缘化路标fill-in最小边缘化旧帧fill-in大→优先边缘化路标5.4 线性化点锁定问题边缘化同时锁定了被移除变量的线性化点。如果这些变量的线性化在当前更准确的点下是有偏的这部分偏差会被永久保留在先验因子中——无法被后续优化修正。这与EKF不一致性的根源完全相同。实践中的缓解策略DSO的动态边缘化根据运动大小选择边缘化对象VINS-Mono的边缘化策略仅边缘化路标fill-in最小线性化点锁定影响小定期重置滑动窗口丢弃先验中的累积线性化误差6. 三种后端策略的统一对比6.1 方法谱系三种SLAM的后端构成了一个从纯滤波到纯优化的连续谱纯滤波 ←——————————————————————→ 纯优化 │ │ │ EKF-SLAM RBPF(GMapping) 全BA(视觉) │ IEKF(FAST-LIO2) │ │ 滑窗BA(DSO/VINS) │ │ 位姿图(Cartographer) │ └─ 状态恒定 ──── 有限增长 ──── 全历史 ─┘6.2 选择后端的决策因素因素倾向滤波倾向优化状态维度低SE(2)的3维高SE(3)路标的数千维传感器信噪比高激光→ EKF线性化足够好低视觉→ 需要重线性化修正回环需求不需要全局一致性需要AR/导航等需要准确的地图计算平台嵌入式/低功耗桌面/服务器观测模型非线性度低几何距离→近似线性高透视投影→强非线性6.3 核心论点滤波和优化的本质差异不在于框架本身两者都在向信息矩阵中添加因子而在于滤波每次添加因子后立即边缘化全部历史——状态恒定但线性化误差不可逆优化保留全部或部分历史——可以重线性化修正误差但计算量增长2D激光能使用滤波RBPF是因为低维状态空间SE(2), 3维使得全历史保留优化和部分历史保留RBPF的精度差异不大——激光的高信噪比导致了滤波已足够好的局面。视觉必须使用优化是因为高维状态空间低信噪比→单次线性化的误差不可接受→需要反复重线性化→优化是唯一途径。本文总结三种SLAM的后端方法由传感器特性和状态维度共同决定2D激光低维高信噪比→RBPF可行→ 3D激光中维高信噪比→IEKF/因子图并行→ 视觉高维低信噪比→必须BARBPF在2D激光中有效的三个前提低维状态空间SE(2), 3维、高精度观测激光~1cm、改进提议分布GMapping先做ICP缩小采样范围EKF不一致性的根源线性化点漂移 历史线性化误差无法修正——IEKF通过在后验均值处迭代更新缓解了这一问题FAST-LIO2的IEKF 滤波和优化之间的桥梁——迭代重线性化使滤波性能接近优化但仍无法修正已边缘化的历史视觉必须用BAn ≈ 5000 n \approx 5000n≈5000个路标 × EKF的O ( n 2 ) O(n^2)O(n2)协方差更新 数学上完全不可行Schur消元稀疏BA将复杂度降为O ( m 3 n ) O(m^3 n)O(m3n)滑窗 精度vs效率的工程折中边缘化保留信息但引入fill-in——三种SLAM根据状态维度选择不同的边缘化策略标签SLAMRBPFEKFIEKF滑窗优化后端
SLAM后端：滤波与滑窗优化的理论分析
发布时间：2026/5/26 22:19:09
专栏系列2D/3D/视觉SLAM理论详解共10篇 |难度高级 |预计阅读28分钟前置知识贝叶斯滤波第1章、非线性优化第2章、图优化理论第5章本文定位第6篇对比三种SLAM后端方法——从2D激光的RBPF到视觉的滑窗优化摘要本章从理论层面比较三种SLAM后端的两大阵营——滤波方法和优化方法——在2D激光、3D激光和视觉中的不同应用。重点揭示(1) 为什么RBPF在2D激光SLAM中有效但在视觉中不可行(2) EKF不一致性的数学根源(3) 现代3D激光SLAM中的IEKFFAST-LIO2如何通过迭代更新缓解不一致性(4) 滑窗优化和边缘化在三种SLAM中的不同应用策略。目录1. 滤波与优化两种后端的哲学差异2. 2D激光后端RBPF与GMapping3. EKF-SLAM与3D激光中的IEKF4. Visual SLAM的后端演化5. 滑窗优化的理论基础6. 三种后端策略的统一对比1. 滤波与优化两种后端的哲学差异1.1 处理历史信息的方式滤波方法Filtering维护当前时刻的状态后验通过边缘化丢弃所有历史变量每个时刻执行 predict → update → marginalize 的三步循环历史线性化误差无法修正一旦边缘化历史信息被压缩且不可逆优点状态规模恒定适合严格的在线实时估计优化方法Smoothing/Optimization维护一段窗口内的完整状态所有位姿路标在新信息到来时重新优化整个窗口可以回头修正过去的估计回环修正的本质优点精度远高于滤波代价状态规模随时间增长1.2 三种SLAM中的后端选择2D激光3D激光视觉经典滤波方法RBPFGMappingIEKFFAST-LIO2EKF-SLAM早期现代优化方法位姿图优化Cartographer因子图优化LIO-SAMBAORB-SLAM3混合/特殊CSMBBCartographer前端滑窗优化滑窗BA主流趋势滤波→优化过渡中IEKF仍占据重要地位优化完全取代滤波关键问题为什么滤波在2D/3D激光SLAM中仍然占有一席之地但在视觉SLAM中几乎被优化完全取代2. 2D激光后端RBPF与GMapping2.1 Rao-Blackwellization分解GMapping的核心是RBPFRao-Blackwellized Particle Filter。它利用了2D激光SLAM的一个关键条件独立性给定完整的机器人轨迹x 1 : t \mathbf{x}_{1:t}x1:t栅格地图的各栅格的后验估计是条件独立的。数学表述为p ( x 1 : t , m ∣ z 1 : t , u 1 : t ) p ( x 1 : t ∣ z 1 : t , u 1 : t ) ⏟ 粒子滤波估计轨迹 ⋅ ∏ c ∈ cells p ( m c ∣ x 1 : t , z 1 : t ) ⏟ 独立估计每个栅格 p(\mathbf{x}_{1:t}, \mathbf{m} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \underbrace{p(\mathbf{x}_{1:t} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t})}_{\text{粒子滤波估计轨迹}} \cdot \prod_{c \in \text{cells}} \underbrace{p(\mathbf{m}_c \mid \mathbf{x}_{1:t}, \mathbf{z}_{1:t})}_{\text{独立估计每个栅格}}p(x1:t,m∣z1:t,u1:t)粒子滤波估计轨迹p(x1:t∣z1:t,u1:t)⋅c∈cells∏独立估计每个栅格p(mc∣x1:t,z1:t)FastSLAM对应的是路标的条件独立性给定轨迹后各路标独立GMapping对应的是栅格的条件独立性。2.2 为什么RBPF在2D激光中有效RBPF在2D激光SLAM中运作良好的三个条件(1) 低维状态空间轨迹是SE(2)3维。每个粒子仅需维护3维的轨迹变量——粒子滤波在低维空间中有效在高维空间中如视觉SLAM的6维数百个路标会遭遇维度灾难。(2) 高精度观测激光测距精度~1-3cm信噪比极高。高精度观测意味着似然函数p ( z t ∣ x t , m ) p(\mathbf{z}_t \mid \mathbf{x}_t, \mathbf{m})p(zt∣xt,m)高度集中——绝大多数粒子的权重接近零仅少数粒子匹配当前观测。这使得粒子滤波能高效地收敛。(3) 提议分布改进GMapping的关键贡献是改进的提议分布。标准RBPF从里程计运动模型采样提议分布宽导致大多数样本在似然函数下权重极低。GMapping先用扫描匹配ICP得到一个高斯近似的最优建议分布x t ∼ N ( x t ICP , Σ ICP ) \mathbf{x}_t \sim \mathcal{N}(\mathbf{x}_t^{\text{ICP}}, \mathbf{\Sigma}^{\text{ICP}})xt∼N(xtICP,ΣICP)从中采样。这使得有效样本数大幅提升——同等粒子数下覆盖的模式更多。2.3 RBPF vs 位姿图优化的权衡RBPF (GMapping)位姿图优化 (Cartographer)地图类型栅格地图每粒子独立地图子图全局位姿图回环处理隐式粒子重采样后回溯显式回环检测→位姿图优化→重新拼接子图计算量O ( M ⋅ N ) O(M \cdot N)O(M⋅N)M MM粒子数O ( ( 3 K ) 3 ) O((3K)^3)O((3K)3)K KK节点数内存高每个粒子独立地图中子图位姿图大规模适用性差粒子数不随面积增长好位姿图规模随面积线性增长全局一致性较差不能真正修正历史好回环后全局优化Cartographer最终取代GMapping成为2D激光SLAM的主流正是因为位姿图优化在处理大规模地图和回环修正上的压倒性优势。3. EKF-SLAM与3D激光中的IEKF3.1 EKF-SLAM的不一致性根源EKF-SLAM最严重的问题是不一致性Inconsistency——总是低估真实的不确定性过于自信。三个理论根源Huang Dissanayake, 2006(1) 线性化点漂移Linearization Point DriftEKF中均值每帧更新后变化但协方差矩阵中的块是在旧的线性化点处计算得到的。当均值移动后这些旧线性化点不再准确——基于它们的协方差无法正确反映新均值处的不确定性。(2) 观测模型的非线性视觉SLAM的透视投影和3D激光的点到线/面距离都是非线性函数。EKF的一阶泰勒展开在非线性强的区域产生系统性的线性化偏差这个偏差在每次更新中累积。(3) 为什么优化方法不存在这个问题图优化方法在每次迭代中在当前的线性化点重新评估所有残差和雅可比。关键是可以回到同一个变量和不同的线性化点重新做线性化——EKF一旦边缘化历史位姿就永久性地失去了重新线性化的能力。3.2 FAST-LIO2的IEKF迭代缓解不一致性FAST-LIO2使用**迭代扩展卡尔曼滤波IEKF**来缓解EKF的不一致性标准EKF在当前均值处做一次线性化和一次更新。IEKF在更新步中做多次迭代。每次迭代中在当前已更新的均值处重新计算观测模型的雅可比H k \mathbf{H}_kHk用新的H k \mathbf{H}_kHk重新计算卡尔曼增益和更新量重复直到收敛这相当于在后验均值处做线性化而非先验均值处减少了线性化误差。IEKF与优化的等价性IEKF的迭代更新等价于在当前状态处做一个高斯-牛顿迭代——它用后验均值作为新的线性化点。这在一定程度上弥合了滤波和优化之间的理论鸿沟。但IEKF仍然不如全优化因为它继承了滤波的边缘化不可逆问题——一旦某帧被边缘化它的线性化点就被永久锁定。3.3 为什么IEKF在3D激光中保持竞争力因素对IEKF有利对位姿图优化有利实时性要求严格恒定时间每次更新量固定时间随图增大而增长回环处理困难需要数据回溯自然处理计算平台嵌入式MCU可运行需要较强的CPU精度要求局部里程计精度足够全局一致性要求高FAST-LIO2的IEKF在局部里程计无回环中提供与全优化相当甚至更好的精度和速度。但当需要全局一致性和回环修正时因子图优化LIO-SAM的架构更优。4. Visual SLAM的后端演化4.1 从EKF到BA的范式转换视觉SLAM的后端经历了从滤波到优化的范式转换2000-2008EKF-SLAMDavison, 2003。单目EKF-SLAM状态向量包含相机位姿所有路标。随着路标从数十增长到数千O ( N 2 ) O(N^2)O(N2)的协方差更新变得不可行3000个路标→9000维协方差→每步更新~8 × 10 7 8 \times 10^78×107次运算。2007-2014PTAM的分叉架构 BA。PTAM证明了BA可以在普通PC上实时运行。Keyframe-based BA仅优化关键帧大幅减少了优化频率和规模。2015-至今ORB-SLAM3的全BA 位姿图优化。局部BA共视图内 全局位姿图回环后 全BA回环精化的三层优化策略。4.2 为什么视觉SLAM必须使用优化视觉SLAM不使用滤波的根本原因不是精度——而是路标的数量级2D激光RBPF粒子维护的是栅格地图固定的分辨率数组O ( M ⋅ cells ) O(M \cdot \text{cells})O(M⋅cells)不随路标数增长3D激光IEKF状态向量仅含位姿偏置少数活跃特征O ( 20 2 ) 400 O(20^2) 400O(202)400维视觉EKF-SLAM状态向量含所有路标O ( n 2 ) O(n^2)O(n2)复杂度n ≈ 5000 n \approx 5000n≈5000→ 完全不实时视觉BA的O ( m 3 n ) O(m^3 n)O(m3n)复杂度经过Schur消元使得在5000路标下仍然可实时求解。5. 滑窗优化的理论基础5.1 滑窗优化 精度与效率的折中滑窗优化是在全批图优化精度最高、计算量随窗口增大和滤波计算量恒定、精度受限之间的工程折中。维护最近k kk帧k kk通常7-15每新帧到来时优化窗口内所有变量位姿路标/偏置边缘化超出窗口的最老变量通过Schur补保留先验信息5.2 三种SLAM中的滑窗策略2D激光3D激光视觉是否使用滑窗通常不使用是是窗口大小∞全历史50-100帧7-15关键帧边缘化对象—最老帧最老帧 或 旧路标Fill-in管理—定期重置窗口边缘化路标而非帧5.3 边缘化 信息保留 Fill-in代价边缘化通过Schur补保留被移除变量的历史信息。代价是引入填充Fill-inp ( x in ) ≈ N ( x ^ in , Σ in − Σ in,old Σ old,old − 1 Σ old,in ) p(\mathbf{x}_{\text{in}}) \approx \mathcal{N}(\hat{\mathbf{x}}_{\text{in}}, \mathbf{\Sigma}_{\text{in}} - \mathbf{\Sigma}_{\text{in,old}}\mathbf{\Sigma}_{\text{old,old}}^{-1}\mathbf{\Sigma}_{\text{old,in}})p(xin)≈N(x^in,Σin−Σin,oldΣold,old−1Σold,in)三种SLAM中fill-in的可接受度差异2D激光不边缘化→无此问题每帧仅3维的全历史保留可行3D激光每帧15维50-100帧滑窗→fill-in是主要瓶颈→定期重置窗口丢弃先验视觉路标的3×3对角块使得边缘化路标fill-in最小边缘化旧帧fill-in大→优先边缘化路标5.4 线性化点锁定问题边缘化同时锁定了被移除变量的线性化点。如果这些变量的线性化在当前更准确的点下是有偏的这部分偏差会被永久保留在先验因子中——无法被后续优化修正。这与EKF不一致性的根源完全相同。实践中的缓解策略DSO的动态边缘化根据运动大小选择边缘化对象VINS-Mono的边缘化策略仅边缘化路标fill-in最小线性化点锁定影响小定期重置滑动窗口丢弃先验中的累积线性化误差6. 三种后端策略的统一对比6.1 方法谱系三种SLAM的后端构成了一个从纯滤波到纯优化的连续谱纯滤波 ←——————————————————————→ 纯优化 │ │ │ EKF-SLAM RBPF(GMapping) 全BA(视觉) │ IEKF(FAST-LIO2) │ │ 滑窗BA(DSO/VINS) │ │ 位姿图(Cartographer) │ └─ 状态恒定 ──── 有限增长 ──── 全历史 ─┘6.2 选择后端的决策因素因素倾向滤波倾向优化状态维度低SE(2)的3维高SE(3)路标的数千维传感器信噪比高激光→ EKF线性化足够好低视觉→ 需要重线性化修正回环需求不需要全局一致性需要AR/导航等需要准确的地图计算平台嵌入式/低功耗桌面/服务器观测模型非线性度低几何距离→近似线性高透视投影→强非线性6.3 核心论点滤波和优化的本质差异不在于框架本身两者都在向信息矩阵中添加因子而在于滤波每次添加因子后立即边缘化全部历史——状态恒定但线性化误差不可逆优化保留全部或部分历史——可以重线性化修正误差但计算量增长2D激光能使用滤波RBPF是因为低维状态空间SE(2), 3维使得全历史保留优化和部分历史保留RBPF的精度差异不大——激光的高信噪比导致了滤波已足够好的局面。视觉必须使用优化是因为高维状态空间低信噪比→单次线性化的误差不可接受→需要反复重线性化→优化是唯一途径。本文总结三种SLAM的后端方法由传感器特性和状态维度共同决定2D激光低维高信噪比→RBPF可行→ 3D激光中维高信噪比→IEKF/因子图并行→ 视觉高维低信噪比→必须BARBPF在2D激光中有效的三个前提低维状态空间SE(2), 3维、高精度观测激光~1cm、改进提议分布GMapping先做ICP缩小采样范围EKF不一致性的根源线性化点漂移 历史线性化误差无法修正——IEKF通过在后验均值处迭代更新缓解了这一问题FAST-LIO2的IEKF 滤波和优化之间的桥梁——迭代重线性化使滤波性能接近优化但仍无法修正已边缘化的历史视觉必须用BAn ≈ 5000 n \approx 5000n≈5000个路标 × EKF的O ( n 2 ) O(n^2)O(n2)协方差更新 数学上完全不可行Schur消元稀疏BA将复杂度降为O ( m 3 n ) O(m^3 n)O(m3n)滑窗 精度vs效率的工程折中边缘化保留信息但引入fill-in——三种SLAM根据状态维度选择不同的边缘化策略标签SLAMRBPFEKFIEKF滑窗优化后端
