IEEE Transactions on Robotics

DynoSAM: Open-Source Smoothing and Mapping Framework for Dynamic SLAM

作者：Jesse Morris, Yiduo Wang, Mikolaj Kliniewski, Viorela Ila · 机构：Australian Centre For Robotics (ACFR), School of Aerospace, Mechanical and Mechatronic Engineering (AMME), University of Sydney, Sydney, Australia

会议/期刊：IEEE Transactions on Robotics · 发布日期：2026 · 页码：1178-1197

运动规划优化定位建图状态估计

精读笔记

Problem Setting

论文标题：DynoSAM: Open-Source Smoothing and Mapping Framework for Dynamic SLAM（IEEE Transactions on Robotics / 2026）。

这篇论文实际处理的是 dynamic object-aware SLAM 的后端建模问题：在估计相机轨迹和静态地图的同时，把动态物体的 motion / pose / structure 纳入同一个 smoothing problem。它不是简单地“在动态环境中剔除 moving objects 以提升 VO”，而是反过来问：既然动态物体已经被观测到，如何把它们作为约束源而不是 outlier 使用。

真正困难点在于 object motion 的参考系和物理含义。对于刚体，同一物体上不同点共享一个物理运动，但如果把运动写在 object-local frame 或任意 body frame 中，motion 的数值会依赖 object frame 的定义。许多 pose-based formulation 看似在估计 object pose，实际上没有显式保证动态点遵守正确的刚体运动学。结果是：变量更少、直觉更自然，但优化问题里编码的不是同一个物理量。

关键矛盾是：downstream 需要 object pose 和 trajectory，但最稳定、最 frame-invariant 的观测量其实是物体在外部参考系中的 observed motion。已有方法要么直接估 pose 但运动学约束不干净，要么估 motion 但 object pose 需要后处理恢复。DynoSAM 的目标就是把这个矛盾系统化，并提供可比较的实现框架。

Motivation

作者的动机来自两个缺口。

第一，dynamic SLAM 领域长期缺少 consensus formulation。VDO-SLAM / Henein 系列强调 SE(3) object motion；DynaSLAM II 等直接估 object pose；MVO、AirDOS、ClusterSLAM 又各自有不同 frame convention 和 evaluation protocol。由于 object frame 可任意定义，不同论文报的 object pose / motion error 往往不是在比较同一个量。这个问题比实验表面上更严重：它会导致某些方法看起来估计了“物体运动”，但其误差受 frame placement 和 world-origin distance 污染。

第二，开源、可复现、可插拔的 dynamic SLAM 后端很少。很多工作只发布系统结果，不方便隔离 formulation 本身的影响。DynoSAM 的动机不是提出一个更强前端，而是建立一个 factor-graph 框架，让不同 dynamic SLAM formulation 能在同一 front-end、同一数据、同一 metric 下测试。

作者的核心观察是：如果以 observed motion 而不是 local object motion 作为基本状态，刚体约束可以自然写成所有动态点共享同一个 H；同时 object pose 也可以通过 pose difference 派生出同一个 H。这个观察把 motion estimation 和 pose estimation 两条路线连接起来。

Core Idea

核心思想可以压缩为一句话：把动态物体的运动建模为 world-centric observed motion，并用它统一约束动态点、object pose 和 object trajectory。

在 WCME 中，物体状态直接是 observed motion H_{k-1,k}，动态点满足 m_k = H m_{k-1}。这个约束的好处是它不依赖 object frame 怎么放，也不要求知道物体中心、朝向或 CAD prior。只要同一物体上的点被正确关联，它们就共同约束一个 SE(3) 运动。这是一个很强但合理的 inductive bias：刚体上的所有点共享同一个外部观察到的 motion。

在 WCPE 中，作者没有退回传统 object-centric pose graph，而是把 object pose L_k 作为变量，同时把 H = L_k L_{k-1}^{-1} 代入动态点运动残差。这样 downstream 可以直接得到 object pose，而后端仍然优化 observed-motion-consistent 的几何约束。这个细节是论文真正的新 formulation：不是“多估一个 pose”，而是让 pose estimation 继承 world-centric rigid-body motion 的物理语义。

与 prior 的本质区别是：它不把 object frame 当作运动定义的基础，而是把 frame-invariant observed motion 当作基础，再按需恢复 pose。这个顺序很重要。

Method

1. World-centric observed motion residual：解决 dynamic points 如何形成一致几何约束的问题。对每个物体，跨帧匹配点通过同一个 H 连接。必要性在于 dynamic points 不能像 static landmarks 一样固定在 world 中，但它们也不是独立 scene flow；刚体假设要求它们共享低维 SE(3) 结构。核心变化是把动态观测从 outlier 变成强约束。

2. WCME：直接估 object motion、camera pose、static/dynamic point structure。它解决 motion-first 的 dynamic SLAM 问题，适合评价 object dynamics。其代价是 object pose 不是一等变量，需要从初始 pose 和 motion chain 恢复；这会引入 object pose anchor 的任意性。

3. WCPE：直接估 object pose，但通过 L_k L_{k-1}^{-1} 构造 observed motion factor。它解决 downstream 需要 pose 的问题，同时避免直接 object-centric formulation 的运动学歧义。核心变化是 pose 只是参数化，真正的约束仍是 observed motion。

4. Constant-motion smoothing：解决短时欠约束和噪声导致的 motion jitter。它不是本文最核心创新，更像 practical prior。文中也承认 world-frame smoothing 存在尺度效应，说明这是工程上必要但理论上不完全优雅的部分。

5. Motion Error metric：解决 object motion evaluation 不可比的问题。ME 把 estimated observed motion 转到 ground-truth object frame 后再比较，避免 object frame arbitrary placement 和 world-origin scaling。这个 metric 贡献很实在，甚至可能比部分系统实现更值得迁移。

Key Insight / Why It Works

这篇论文最关键的 insight 是 representation alignment：把优化变量的数学表示和刚体运动的物理不变量对齐。许多 dynamic SLAM 失败不是因为优化器不够强，而是 residual 里写的 motion 不是所有点共享的那个 motion。DynoSAM 的 observed-motion formulation 让“同一物体上的点共享一个 SE(3)”成为显式约束，所以在稀疏/中等密度动态点上也能获得稳定 object motion。

WCPE 成立的原因也在这里：直接估 pose 本身并不会带来正确运动学；关键是 pose 之间的差必须以 observed motion 的形式进入点运动残差。换言之，论文的核心不是 pose vs motion，而是 local-motion vs observed-motion。只要后者被保留，参数化差异影响就小；这也解释了实验中 WCME 和 WCPE 表现接近。

最可能的核心贡献：observed motion 的系统化推导、WCPE 的 pose-parametrized but motion-consistent factor、以及 frame-agnostic ME metric。它们是机制层面的贡献。

较可能只是辅助的部分：前端的 YOLOv8/RAFT、joint optical-flow refinement、object motion refinement、多线程 tracking、batch/sliding-window 工程组织。这些提升系统可用性，但不是概念突破。尤其 VDO-SLAM 和 WCME formulation 很接近，结果差异很可能混合了 tracking quality、dense dynamic features、solver、数据处理和 outlier rejection 的影响；增益来源不清。

这不是 scaling 方法，也不是学习型泛化；它的收益主要来自 better inductive bias 和 evaluation alignment。它把 dynamic SLAM 从“靠前端分割后各自估计”推向“把动态刚体作为可优化结构”的范式。泛化能力来自几何假设，而不是数据覆盖。

Relation To Prior Work

这篇论文最接近 Henein / VDO-SLAM 的 motion-centric dynamic SLAM 谱系，也继承了 GTSAM/SAM 式 smoothing-and-mapping 的建模风格。WCME 本质上不是全新思想，而是对已有 world-centric motion formulation 的工程化、开源化和系统评测。

相对 DynaSLAM II、DSP-SLAM、BodySLAM++、DynaQuadric 等 pose/object-model 路线，DynoSAM 的不同点在于它不依赖强 object prior、shape prior、category-specific motion prior 或 object-centric landmark frame。它把 object motion 写成所有动态点共享的 observed motion，因此更 general，但也更依赖正确的 rigid segmentation 和 point association。

相对 MVO、AirDOS 等多运动/动态对象估计路线，它更强调 world-frame consistency 和 reference-frame correctness。AirDOS 扩展到 articulated human segment motion，DynoSAM 目前仍是 rigid-body 主线。

看似新的地方中，open-source framework、ROS2、前端 pipeline、downstream demo 更多是系统整合；实质创新在于 WCPE formulation 和 ME metric。论文真正新增的信息是：如果你要直接估 object pose，也不应该放弃 observed-motion constraint；如果你要比较 object motion，不能直接比较任意 object-frame RPE 或 world-frame H。

Dataset / Evaluation

评估覆盖面比较充分：既有 KITTI tracking 这种 outdoor driving，也有 OMD 的室内 Vicon 动态刚体，Outdoor Cluster 仿真，TartanAir Shibuya 的大量非刚体动态人群，以及 VIODE 的强遮挡和 VIO 场景。对于证明“framework 能处理多类动态环境”和“camera pose 不会因引入动态物体而崩”是够的。

对核心 claim——object motion estimation 更准——实验总体支持，但不是完全干净。问题在于可比较系统有限，很多第三方方法闭源或只报告部分对象/部分序列；不同系统前端、输入、tracking、object ID 和数据预处理不统一。作者尽量通过统一数据跑 VDO-SLAM、引入 ME metric 来修正，但并不能完全隔离 formulation 本身的贡献。

ME metric 的引入很关键，它确实验证了 prior evaluation 的一个盲点。不过现有方法没有原生报告 ME，部分比较依赖作者重新运行或有限结果，因此 benchmark 仍有 incomplete comparison 的问题。

camera pose 结果说明 DynoSAM 至少没有牺牲 egomotion，在 RPE 上表现强；但 ATE 上并非全面最优，尤其 DynaVINS 这类更专注鲁棒 VIO/动态剔除的方法在强遮挡场景仍有优势。downstream reconstruction 和 trajectory prediction 更像 plausibility demo，不足以证明长期规划能力。

Limitation

第一，核心前提是 object instance segmentation、motion segmentation、dense depth、optical flow 和 temporal object association 足够可靠。论文把主要贡献放在后端是合理的，但实际系统的失败边界很大程度由前端决定。若 object ID 断裂、mask 混入背景、光流错误或深度噪声大，刚体因子会强行优化错误约束。

第二，刚体假设是硬约束。对 cyclist 这类 semi-rigid object 能工作，是因为存在 dominant rigid motion；这不等于方法真正解决了 nonrigid / articulated motion。对人、动物、可变形物体，当前 formulation 只是近似。

第三，constant-motion smoothing 是弱而粗糙的 motion prior。它对局部平滑有用，但不能表达加速度、交互、意图或交通规则。所谓 downstream prediction 并不是 SLAM 后端形成了长期动态理解，而是把估计轨迹喂给另一个预测模型。

第四，scalability 仍是硬问题。full-batch runtime 很重，sliding-window 也远未实时。factor graph 中 ternary/quaternary dynamic factors 和大量动态 landmarks 会让长期、多物体场景迅速膨胀。文中提出 incremental inference 作为未来工作，说明当前更像研究平台而非部署级系统。

第五，WCPE 对 object pose 初始化敏感，而 object pose frame 又天然任意。虽然 observed motion 降低了 frame-dependence，但 direct pose estimation 的 downstream utility 仍依赖 pose anchor 是否语义一致。文中未充分说明如何在真实系统中获得稳定、语义有意义的 object frame。

第六，增益归因不完全清楚。相对 VDO-SLAM 的提升可能来自更好的 front-end、dense tracks、refinement、solver、robust cost、数据处理，而不完全是 formulation。论文对此有一定诚实讨论，但没有充分 ablation。

Takeaway

  • 1. Dynamic SLAM 的核心不是“保留动态点”，而是要把动态点组织成正确的低维运动结构；observed motion 是比 object-local motion 更稳的基本表示。
  • 2. 如果下游需要 object pose，最好的做法可能不是回到 object-centric pose formulation，而是在 pose 参数化中显式保留 observed-motion factor。
  • 这个设计可以迁移到 object-level mapping、dynamic NeRF/SLAM、multi-body BA 等问题。
  • 3. 评价 dynamic object motion 时必须处理 object frame 任意性。

一句话总结

DynoSAM 是一篇把 dynamic SLAM 从工程式动态物体处理推进到 reference-frame-correct 的 factor-graph 建模与评测平台的工作，真正贡献在于 observed-motion-centric formulation、pose-consistent 扩展和 object-motion 评价标准。