做视觉SLAM的人，多半都有过这种经历：实验室里跑得飞起的demo，一搬到地铁站、商场这种人多的地方，轨迹就开始画龙。移动的行人、旋转的推拉门、飞过的球——任何一个出现在画面里的动态物体，都可能让相机的位姿估计在一帧之内崩掉。这不是算法不够努力，而是整个视觉SLAM体系自带的静态世界假设，在真实世界里天然不成立。
SOF-SLAM这篇2019年发表在JCR Q1期刊（影响因子4.076）上的工作，就是冲着“动态环境下的位姿鲁棒性”来的。它把语义分割和几何校验放在一起做双重判断，在公开的动态场景数据集上把轨迹误差压低了一个量级左右。对于正在做移动机器人、AR导航，或者搞三维重建又苦于场景里有人走动的同学来说，这篇论文的思路和工程实现，放到今天依然很有参考价值。下面我从问题本质、方法设计、工程实现和实验评测几个角度，把这套方案掰开揉碎聊一遍。
1. 动态物体为什么能把传统视觉SLAM逼到墙角
1.1 静态世界假设：SLAM最脆弱的地基
绝大多数视觉SLAM算法，不管是特征点法的ORB-SLAM2、基于稀疏直接法的DSO，还是半直接法的SVO，体面的前提都写着同一句话：场景中只有相机在动，其他一切世界点都是静止的。这个假设在数学上非常好用——一个静止三维点在不同帧中的投影位置必须满足多视图几何约束，相机位姿可以被一连串这样的约束精确求解。
可一旦假设被打破，事情就麻烦了。行人手臂上的特征点、车辆车身反光点、旋转门上的把手，它们自身也在运动，却依然作为“世界点”参与位姿求解。SLAM系统不知道谁在动谁没动，它只会把这些点一视同仁地丢进三角化、PNP求解和后端优化里。
我最初接触动态场景时犯过一个典型错误：觉得只要特征点够多，少数动态点会被优化过程“平均”掉。实际测下来完全不是这么回事。动态物体往往离相机近、纹理丰富、特征点密度高，在匹配和优化中占的权重一点都不小。一个人从镜头前走过，他身上的数百个特征点会形成一股强大的错误牵引力，直接把轨迹拖偏。
1.2 动态点污染位姿的完整破坏链路
动态点对位姿求解的污染不是一次性事件，而是一条递进的破坏链路。第一步是破坏帧间初始匹配。当行人占据画面主体位置时，上一帧和当前帧的特征匹配大量落在行人身上，匀速运动模型或者参考帧模型给出的初值就开始偏离真实位姿。
第二步是向优化器输入错误约束。视觉SLAM的位姿求解本质上是让重投影误差最小化，动态点携带的“伪对应关系”会产生巨大的残差。虽然有鲁棒核函数（Huber核）可以压制外点，但核函数只对离群明显的点有效，动态点如果能在当前估计位姿下找到某种“凑合”的投影对应，它就不会被当成外点。
第三步最为隐蔽：动态点被三角化成错误地图点，进入局部地图后污染后续所有帧。我们实验室在某真实场景中复现这类问题时，轨迹误差在行人出现的第10帧左右开始明显偏斜，即使行人离开画面，局部地图里已经混入的错误地图点还会继续影响位姿优化，需要好几秒甚至更长的时间才能慢慢纠正回来。这种“毒地图”效应，比单纯的帧间匹配被干扰更致命。
1.3 RANSAC和几何方法为什么顶不住密集动态
很多人面对动态场景，第一反应是“我有RANSAC，怕什么”。RANSAC确实能剔除一部分不一致匹配，但它有两个天然软肋。
一是内点比例假设。RANSAC依赖随机采样找到一组全是内点的最小解集，当动态物体占画面比例很大，比如行人贴着镜头走过，场景中真正的静态点反而成了少数派，RANSAC的采样成功率急剧下降，甚至可能收敛到动态物体自身构造的“伪模型”上。
二是动态物体也可能自洽。一个行人整体向右移动，他身上的点在连续两帧之间的运动模式其实是高度一致的，完全满足刚性变换关系。这类点并不会在几何上表现出矛盾，RANSAC筛不出来，极线约束也拿它没办法。
所以，纯几何方法对付“稀疏散点式动态”还凑合，对付“一大块完整运动物体”就力不从心了。这就是为什么SOF-SLAM这类工作要把几何信息和语义信息结合——几何负责抓“反例”，语义负责提供“先验”，前者管细节，后者管全局。
2. SOF-SLAM的核心思路：语义与几何怎么打配合
2.1 先验动态与几何动态不是一回事
SOF这个名字按这类工作的命名习惯，可以理解为语义（Semantic）与光流/几何（Optical Flow）融合的意思。它的核心洞察在于：语义信息和几何信息回答的是两个不同层面的问题。
语义分割告诉算法的是“这块区域在语义上属于什么类别”，比如人、车、自行车。它给出的是先验动态——这些类别天然具有随时运动的能力，但“有能力运动”不等于“此刻在动”。一个人站在墙边聊天，姿势基本静止，他的特征点其实对位姿估计是无害的，如果因为语义标签是人就一刀切剔除，反而会把一批本来能用的约束浪费掉。
几何校验告诉算法的是“这个特征点此刻是否真的在动”，但它不知道这个点属于谁。一个被人推着走的椅子，几何残差很高，确实是动态点，但椅子的语义标签通常属于静态家具类。如果只信几何，很难解释为什么一把椅子会在两个连续帧之间平移；如果只信语义，又会把静止行人点的有效约束扔掉。
所以SOF的逻辑是：语义给出候选名单，几何做最终裁决，二者互相当对方的“担保人”。这样可以同时避免语义的误杀和几何的漏判。
2.2 光流与极线约束：几何校验的两种实现路径
具体做几何校验时，2019年前后的动态SLAM工作里常用两种手段，SOF这类方案会对二者做组合或对比选型。
第一种是光流方向与极线方向的一致性检查。给定当前帧的位姿估计和特征点深度，可以把地图点重投影到当前帧，得到理论上的投影位置；同时用稀疏光流（比如LK光流）跟踪该特征点的实际运动方向。如果光流方向与极线方向（即由相机运动引起的理论像素移动方向）存在明显夹角，说明该点的像素运动不能由相机自身运动解释，存在独立运动分量，标记为动态。
第二种是对极约束残差检查。对双目或RGB-D系统来说，特征点的深度通常是已知的。利用当前帧估计位姿，静态特征点的重投影误差应在几个像素以内；动态点因为实际空间位置不符，重投影会落在误差很大的位置，或者其匹配对到极线的距离远超阈值。
我实际对比过这两种方式：光流方向的优势是不需要准确深度，单目也能跑；劣势是光流本身容易被遮挡、快速运动和大梯度光照影响。对极约束的优势是理论清晰、阈值直观；劣势是深度噪声大的区域（比如远处、低纹理区）误判率偏高。SOF把两者结合起来用——光流先做粗筛，对极残差再做精判，比我单独只用其中一种要稳得多。
2.3 语义与几何冲突时的裁决逻辑
双重校验里最关键的其实是冲突处理。我把SOF里能抽象出来的裁决逻辑总结为一张表：
| 语义判断 | 几何残差 | 处理策略 | 说明 |
|---|---|---|---|
| 先验动态类 | 高残差 | 直接剔除 | 语义宣判、几何证实的典型动态点 |
| 先验动态类 | 低残差 | 保留 | 静止的行人，留着能当静态约束 |
| 非先验动态类 | 高残差 | 谨慎剔除或降权重 | 被推动的椅子、手推车，要防漏网 |
| 非先验动态类 | 低残差 | 正常使用 | 常规静态地图点 |
这个表格看着简单，但实际参数调起来很费劲。高/低残差的阈值设严了，静止行人会被误杀；设松了，快速运动的行人点又会漏进来。我在复现类似思路时发现，阈值最好做成自适应——根据当前帧的平均残差动态调整判断线，而不是用固定像素值。
另外一个容易被忽视的细节：语义分割的类别列表要按场景配置。室内场景中，“人”是主要的前景可移动目标；室外车载场景中，“车”“卡车”“摩托车”才是重点。SOF论文里采用的公开分割网络能识别几十类目标，实际使用时只挑先验动态类别，其余都默认静止，这样可以大幅降低误判概率和计算量。
3. 基于ORB-SLAM2的系统扩展：语义线程怎么加进来
3.1 为什么选择ORB-SLAM2做底座
SOF这套系统选择ORB-SLAM2作为基座，2019年做动态SLAM的人大多走这条路。原因很简单：ORB-SLAM2是当时最完整、最开源、最容易改的视觉SLAM框架。它支持单目、双目、RGB-D三种输入，拥有跟踪（Tracking）、局部建图（LocalMapping）、回环检测（LoopClosing）三线程架构，后端带局部BA和全局BA，模块封装得够清晰，往里插东西很方便。
更重要的是，ORB-SLAM2用ORB特征点作为整个系统的数据基础，特征点的像素位置、深度、描述子、匹配关系都是显式可查的。这给按“点”做动态标记提供了极大的操作空间——你只需要维护一个跟特征点索引对应的动态标志位数组，就能在匹配、三角化、BA各个阶段分别拦截动态点。
3.2 整体架构改动：异步语义线程的挂载方式
SOF对ORB-SLAM2的改动，核心是新增了一条语义分割线程。这条线程和跟踪线程平行跑，不直接参与位姿计算，只负责输出“图像帧的逐像素语义标签”。
在具体部署时，语义线程要做的事有三件：
- 从跟踪线程接收关键帧或普通帧的图像数据，按时间戳放进输入缓冲队列；
- 跑一次语义分割网络，输出每个像素的类别标签图；
- 将标签图上采样回原始图像尺寸（通常会把网络输入分辨率缩得很小来换取速度），送给跟踪线程查表使用。
这里有个很常见的工程误区：让跟踪线程同步等待语义结果。GPU上跑一次分割网络，哪怕分辨率压缩到320×240，也要几十毫秒到几百毫秒，同步等待的结果就是帧率塌方，系统直接失去实时性。异步才是正解——跟踪线程先用纯几何把位姿算出一版来，语义结果到达后再做动态剔除和二次优化。
3.3 动态点剔除的操作时序：先跟踪、后校验、再优化
我把SOF中动态点剔除的完整时序拆解成了五步，这套流程也是后来很多语义SLAM系统沿用的模板：
- 跟踪线程照常提取ORB特征，完成当前帧与参考帧的匹配，用运动模型或对极约束给出当前帧的初始位姿；
- 从语义线程取回当前帧的标签图，用特征点的像素坐标查标签，筛出落在先验动态类别区域内的候选动态点；
- 对候选动态点跑光流方向和极线残差校验，剔除确实具备独立运动特征的点；同时检查非先验动态类别内是否存在高几何残差的异常点；
- 从当前帧匹配和局部地图观测中删除最终确认的动态点，用剩下的静态点重新做一次PNP解算和局部优化；
- 在后续建图步骤里禁止动态点被三角化成新地图点，已完成三角化的动态地图点也会被移除或标注失效。
这五步里我最想强调第四步“再次优化”的必要性。很多初做动态SLAM的人只在匹配阶段删点，删完直接用原初值继续跑。但初始位姿本身就是被动态点污染过的，带着一个偏掉的初值去优化剩余静态点，结果还是会偏。必须先删点、再重新求解位姿，才能让后续优化真正建立在干净的数据上。
3.4 三个容易被忽略的工程细节
第一个是语义掩膜的膨胀处理。分割网络的边缘预测通常不准，行人和背景交界处的像素可能被错分。直接在原始标签图上按点查询，会把一批贴着人体轮廓的静态背景点误判成动态点。正确做法是先对先验动态类别做形态学膨胀（dilate），让保留区域向内收紧，宁可牺牲一点召回率也要保住精度。
第二个是时间戳对齐。语义线程和跟踪线程之间必须建立严格的帧同步机制。我用的方案是给每一帧图像打上唯一的递增ID，语义结果返回时携带输入帧ID，跟踪线程只消费与当前帧ID匹配的标签数据。否则一旦出现处理延迟，拿上一帧的语义标签去判断当前帧的特征，误判率会高得离谱。
第三个是降级策略。语义分割模型偶尔会抽风——显存不足、推理超时、输入分辨率异常，都会让标签图迟迟不来。系统必须定义一个最大等待时延，超时后自动切换到纯几何模式跑，暂不启用语义先验，而不是傻等。这个降级开关对实际部署的鲁棒性影响很大，我在某个标注密集的动态场景里实测过，没有降级策略的系统平均每跑三分钟就会卡顿一次。
4. 公开数据集上的实测表现：精度提升到底来自哪里
4.1 评测口径：动态场景基准和阿克曼误差怎么看
动态SLAM的评测，业内最常使用的是公开的RGB-D动态场景基准数据，里面包含多个典型室内序列，按动态程度分成低动态、高动态等不同档位。低动态场景一般是两三个人坐着聊天、做小幅动作；高动态场景则是行人在桌子周围来回走动、骑车穿行，甚至有人主动贴近镜头制造大幅度干扰。
评价指标上，最常用的是绝对轨迹误差（ATE）的均方根误差（RMSE），它能直接反映估计轨迹与真值轨迹之间差多少。另一个辅助指标是相对位姿误差（RPE），用来衡量每段短时间内的位姿漂移。前者看全局，后者看局部，两个一起用才能判断一个系统到底是全局偏移还是局部抖动。
我个人的习惯是绝对不只看论文里报的表格数字，还要看轨迹的可视化图。ATE的RMSE只是一个汇总标量，轨迹图能看到误差集中在哪个时段、是不是与行人出现的时间强相关。SOF这类系统在可视化里表现好的序列，往往都是误差峰值被显著削平的序列——那些高峰值的“毛刺”才是动态干扰的直接体现。
4.2 高动态场景的收益：误差量级的真实变化
从同类型方法在公开基准上的实验结果看，ORB-SLAM2基线在高动态序列上的ATE RMSE通常在0.5米到1米以上，部分极端序列甚至直接追踪丢失。引入语义+几何双重剔除后，同一序列的ATE RMSE可以降到2厘米到5厘米量级，误差下降接近一个数量级。低动态序列上也有提升，但幅度没那么夸张，一般从几厘米降到一两厘米。
这个结果其实很合理。高动态序列里，动态点比例高、干扰面积大，几何方法已经顶不住；语义信息恰好能从像素层面直接告诉你“这一整块都是人”，把大批动态点一次性摘掉，等于给了优化器一个极干净的内点集合。低动态场景下，动态干扰本来就小，传统几何外点剔除已经能处理大部分问题，语义信息带来的边际增益自然有限。
值得一提的是，静态场景下的表现。某些语义SLAM系统在纯静态场景里反而比基线差，原因是分割网络偶尔会把静态物体误判成先验动态类，或者几何校验把某些纹理复杂区域的静态点误伤，导致有效约束变少。SOF在静态序列上能维持与原系统持平甚至略优的精度，这得益于它“只删语义和几何双确认的点”的保守策略——宁可多留一些可疑点，也不误杀一个关键静态点。
4.3 计算开销与实时性：语义不是免费午餐
加语义线程必然带来额外开销。哪怕是异步执行，语义分割网络在GPU上的一次推理也需要占用可观的计算资源。我在配置类似的系统时，通常把分割网络输入分辨率选在320×240上下，输出标签图再上采样回原始尺寸。这个设置下，单帧推理时间在GPU上大概几十毫秒，CPU上要慢很多倍。
系统整体帧率会受到影响。ORB-SLAM2原版跑RGB-D输入时，在普通桌面级GPU上能跑到每秒30帧以上；挂上语义线程后，实际帧率取决于语义推理速度和跟踪线程是否被阻塞。异步设计得好，帧率下降幅度有限；设计不好，跟踪线程等待语义结果，帧率直接掉一半。
如果对实时性有硬指标要求，一个常用技巧是“跳帧语义分割”——只对关键帧做语义推理，普通帧沿用最近关键帧的标签图。动态物体的区域变化在几十毫秒内通常不会太大，跳帧带来的误判增量可以接受，但计算负载能降一个档次。SOF的实验里也有类似的策略考量。
5. 从SOF-SLAM看动态SLAM的工程方法论
5.1 “先验加证据”比堆料更稳
SOF这套方案给我最深的感触，是它没有盲目堆算法。它没有为了“更深的语义理解”而上更重的模型，也没有完全抛弃经典的几何校验。语义给先验，几何给证据，两者互为兜底，这才是它稳健的根源。
我后来在另一个导航项目里也沿用了这个模式。那个项目里要用轮式机器人在办公室环境长时间运行，办公室里人来人往。我一开始试图用最新的实例分割模型替代传统语义分割，结果模型确实更强了，但动态点剔除的准确率并没有成比例提升——因为瓶颈在几何校验和整体决策逻辑上，不在标签的精细程度。把几何模块和裁决逻辑调好，收益比单纯换重模型大得多。
5.2 工程落地中的降级与容错设计
学术系统搬到工程环境里，最先考验的往往不是精度，而是异常处理。语义分割模型可能在某个特殊光照下输出大面积噪声；GPU可能被其他任务抢占导致推理超时；数据集里的低纹理区域会让光流跟踪大面积失败。这些情况下，系统不能崩溃，必须优雅降级。
SOF的异步线程架构天然提供了一个降级切入点：语义结果不达预期时，系统可以退回纯几何模式；几何校验点数太少时，可以跳过二次优化直接用初值。模块解耦的意义就在这里——每个模块都能独立失效，但系统整体还能运转。
我在实际调试中还发现一个经验：动态剔除逻辑里要加“最小保留静态点数量”的硬约束。如果某帧被剔除后剩下的静态匹配点少于比如30个，说明剔除策略可能过于激进，这时候宁可恢复一部分可疑点也要保证位姿求解有足够的约束支撑。类似的逻辑在SOF的双校验框架里很容易实现，但对纯语义或纯几何的简化方案就很难做到那么精细。
5.3 从论文到实践的三个直接建议
第一个建议是，如果你的项目场景是固定视角的室内监控，动态SLAM的语义先验可以只保留“人”一个类别；如果是车载场景，重点配置“车”和“行人”。类别列表越精简，误判率越低，推理也越快，不要为了“通用性”把几十类全部启用。
第二个建议是，动态点剔除后，建议在地图点结构里加一个“历史动态标志位”。单个点如果被标记为动态的次数超过某个阈值，直接物理删除并禁止再次三角化。这个机制可以防止同一个动态物体反复进入地图——在移动物体周期性出现的场景里特别管用。
第三个建议是，论文里看看ATE的改善就够了，但工程上一定要关注最坏情况，也就是误差峰值。SOF这类系统真正解决的是“动态干扰导致的大误差尖峰”，把轨迹的最大误差压下来，比把平均误差降低对导航更有意义。你评估自己的系统时，不妨多统计一下ATE大于某个阈值的时间占比，这个指标比RMSE更能反映实际体验。
我后来在多个项目里尝试过带语义的动态SLAM方案，每一次踩坑回看SOF的设计，都会觉得它的核心价值并不在于某个具体的网络或者某一层的创新，而在于那种“不把宝押在单一线索上”的系统设计思路。语义视觉SLAM发展到后来，用上了更强的分割模型、更聪明的物体跟踪、更复杂的概率融合，但语义先验与几何证据互相校验的基本范式，确实是这套工作中值得借鉴的内核。
