Description：WOLF 论文与代码笔记 — 基于因子图的模块化机器人估计框架，Wolf Tree 层次结构、传感器/处理器插件、预积分抽象 pipeline、YAML 自动配置、五类真机验证案例
My Notion Note ID：K2E-B-S19-1
Created：2023-06-08
Updated：2026-06-11
License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

1. Summary

Title: WOLF: A Modular Estimation Framework for Robotics Based on Factor Graphs Authors: Joan Solà, Joan Vallvé, Joaquim Casals, Jérémie Deray, Médéric Fourmy, Dinesh Atchuthan, Andreu Corominas-Murtra, Juan Andrade-Cetto (IRI, CSIC-UPC, Barcelona) Paper: arXiv:2110.12919 (IEEE RA-L 2022) Repo: IRI GitLab — wolf_lib

WOLF 是一个通用模块化估计后端框架，把机器人状态估计（定位、SLAM、传感器融合）统一表述成因子图。GTSAM（Georgia Tech Smoothing and Mapping，基于因子图和贝叶斯树的状态估计库）/g2o（通用图优化库，用稀疏 Schur 补求解位姿/地图点联合 BA）是纯优化库；WOLF 是包含前端（Processor）和后端（Solver wrapper）的完整估计框架，通过 solver wrapper 与 Ceres（Google 开源非线性最小二乘库，支持自动/手动微分和多种稀疏线性求解）/GTSAM/g2o/SLAM++ 等库对等对接——而非构建在它们之上。Core 自带 Ceres wrapper；GTSAM/g2o/SLAM++ 等可另行实现 wrapper。

核心数据结构 Wolf Tree 把估计问题映射到三棵子树（Hardware、Trajectory、Map），所有节点以智能指针互连，因子图自动从中派生。加入新传感器只需写 Sensor + Processor 插件——不动核心代码。

论文以五类真机场景验证：2D 激光建图、港口载运车 GNSS+激光、UAV GNSS+IMU、类人机器人视觉惯性、四足机器人全身状态估计，覆盖 wheeled / aerial / humanoid / legged 平台，估计精度从 cm 到亚厘米级，1 kHz 实时状态输出。

Wolf Tree 三棵子树：Hardware (Sensors/Processors) → Trajectory (Frames/Captures/Features/Factors) → Map (Landmarks)

2. Key Contributions

Wolf Tree：树状实体层级，跨支路指针连接 Sensor→Feature/Factor，使前端/后端信息共享（如激光里程计的关键帧可同时被 IMU 处理器访问）
抽象算法：Motion Pre-integrator（IMU/里程计/接触力统一 pipeline）、Feature/Landmark Tracker、Loop Closer——插件化，与传感器类型解耦
原生传感器自标定：外参、内参、预积分参数均可纳入状态在线估计
帧自动同步：多处理器按时间戳策略共享关键帧，无需手动对齐
YAML 自动配置：无需重编码改变估计问题，只改 YAML

3. Method

3.1 Wolf Tree 层次结构

Problem
 ├── Hardware → Sensor(外参状态) → Processor
 └── Trajectory → Frame → Capture → Feature → Factor
 └── Map → Landmark

Sensor：传感器 + 静态/动态参数 state block（外参在 Sensor 上，内参可选 Capture）
Processor：数据处理单元，生成 Capture / Feature / Factor；Motion Pre-integrator 和 Tracker 是两大抽象子类
Frame：一个时刻的状态（位姿+其他）
Factor：因子图的约束边，含残差函数 $r(x_1,\dots,x_n)$ 及雅可比

MAP 代价函数：估计问题统一为

\mathbf{x}^* = \arg\min_{\mathbf{x}} \sum_i \rho_i\!\left(\left\|\mathbf{r}_i(\mathbf{x})\right\|^2_{\boldsymbol{\Sigma}_i}\right)

$\mathbf{r}_i$ ：第 $i$ 个因子的残差； $\boldsymbol{\Sigma}_i$ ：对应协方差（Mahalanobis 加权）； $\rho_i$ ：鲁棒核（可选）。Wolf Tree 中每个 Factor 对应一个 $\mathbf{r}_i$ 。

3.2 Motion Pre-integrator Pipeline

适用于 IMU、轮式里程计、接触力积分等：

\mathbf{v} = f(\mathbf{u},\bar{\mathbf{c}}), \quad \boldsymbol{\delta} = g(\mathbf{v}), \quad \bar{\boldsymbol{\Delta}} \leftarrow \bar{\boldsymbol{\Delta}} \circ \boldsymbol{\delta} \tag{1}

$\mathbf{u}$ ：原始测量； $\bar{\mathbf{c}}$ ：标定参数估计； $\boldsymbol{\delta}$ ：单步 delta； $\bar{\boldsymbol{\Delta}}$ ：累积预积分量； $\circ$ ：预积分流形复合运算（将新 delta 右乘到累积量上，在旋转等非欧流形上正确传播）。

协方差传播：

\mathbf{Q}_\Delta \leftarrow \mathbf{J}^\Delta_\Delta\,\mathbf{Q}_\Delta\,(\mathbf{J}^\Delta_\Delta)^T + \mathbf{J}^\Delta_\delta\,\mathbf{J}^\delta_\mathbf{v}\,\mathbf{J}^\mathbf{v}_\mathbf{u}\,\mathbf{Q}_\mathbf{u}\,(\cdots)^T \tag{2}

雅可比符号 $\mathbf{J}^A_B \triangleq \partial A/\partial B$ （同 §1 符号）： $\mathbf{J}^\Delta_\Delta$ = 新旧累积量间的转移雅可比； $\mathbf{J}^\Delta_\delta$ = 累积量对单步增量 $\boldsymbol{\delta}$ 的雅可比； $\mathbf{J}^\delta_\mathbf{v}$ = $\boldsymbol{\delta}$ 对中间量 $\mathbf{v}$ 的雅可比（ $\mathbf{v} = f(\mathbf{u}, \bar{\mathbf{c}})$ ）； $\mathbf{J}^\mathbf{v}_\mathbf{u}$ = $\mathbf{v}$ 对原始测量 $\mathbf{u}$ 的雅可比； $\mathbf{Q}_\mathbf{u}$ = 测量噪声协方差。第二项展开即通过链式法则把测量噪声传播到累积量协方差。

标定参数雅可比： $\mathbf{J}^\Delta_\mathbf{c} \leftarrow \mathbf{J}^\Delta_\Delta\,\mathbf{J}^\Delta_\mathbf{c} + \mathbf{J}^\Delta_\delta\,\mathbf{J}^\delta_\mathbf{v}\,\mathbf{J}^\mathbf{v}_\mathbf{c}$

高频状态输出（用于控制）：

\mathbf{x}_t = \mathbf{x}_i \boxplus \bar{\boldsymbol{\Delta}}_{it} \tag{3}

$\boxplus$ ：流形加法（boxplus retraction），将切空间增量 $\bar{\boldsymbol{\Delta}}_{it}$ 映射回流形上的状态 $\mathbf{x}_t$ ；对 $SO(3)/SE(3)$ 退化为指数映射 + 四元数乘法。kHz 量级计算量，适合嵌入式 / 实时控制。

3.3 传感器自标定（三策略）

区分静态/动态，与外参/内参正交：

静态参数（固定外参、相机内参等）→ Sensor state block，优化时始终可见
动态参数（IMU bias、时变外参等）→ Capture state block，随时间变化
预积分参数标定：标定量通过一阶展开修正而无需重积分： $\boldsymbol{\Delta}(\mathbf{c}) = \bar{\boldsymbol{\Delta}} \oplus \mathbf{J}^\Delta_\mathbf{c}(\mathbf{c}-\bar{\mathbf{c}})$ （ $\oplus$ ：切空间扰动加法，在流形切空间上将线性修正叠加到名义预积分量）

3.4 因子图可视化

因子图：红色方块 = 地标因子（连帧 F1-F3 与地标 E），蓝色方块 = 运动因子（相邻帧 IMU/里程计约束）

4. Experiments & Results

五个真机验证案例（非基准数字比较，重点展示框架通用性）：

案例	传感器	平台	关键精度
Demo	2D LIDAR + 轮式里程计	Turtlebot	室内地图连续闭合
LOGIMATIC	GNSS + 轮式里程计 + 4×LIDAR	8 轮港口载运车（~1000×500 m）	GNSS TDCP（Time-Differenced Carrier Phase，载波相位差分，消公共误差后 cm 级精度）
GAUSS	2× GNSS + 低成本 IMU	UAV (M600)	伪距 ~1 m；TDCP ~cm
MEMMO-vision	相机 + IMU（AprilTag）	HRP-2 类人机器人	漂移 ≤ 4 cm，200 Hz 输出
MEMMO-forces	IMU + 关节编码器 + 电机电流	Solo12 四足机器人	全身状态估计 1 kHz，延迟 < 1 ms

2D LIDAR 建图：Turtlebot 室内环境连续激光扫描地图（绿色轮廓线）

LOGIMATIC 港口场景：载运车（straddle carrier）点云地图 + 集装箱检测（鸟瞰）

5. Ablation & Discussion

WOLF 论文无传统消融实验（重点是框架展示，非单项指标比较）。以下是设计权衡讨论：

Wolf Tree vs 直接用 GTSAM/g2o：Wolf Tree 提供跨支路信息共享（如 Processor A 的关键帧可被 Processor B 访问），GTSAM/g2o 是纯图优化库，需应用层自行管理这些连接
多线程策略：Solver 独立线程 + 每个 ROS publisher 独立线程 + 每 Processor 单线程（当前实现）——并行程度有限，未来可扩展
YAML 配置 vs 硬编码：灵活性换来运行时类型检查难度增加，配置错误只在运行时暴露

6. Strengths / Limitations / Future Work

优点：

真正模块化：加传感器 = 写插件，不改核心
覆盖传感器类型极广（视觉、IMU、激光、力矩、GNSS）
原生支持传感器自标定，工程完整
YAML 配置简化部署

局限：

Core 只自带 Ceres wrapper；GTSAM/g2o 需自行实现 wrapper
当前 Processor 单线程，高频多传感器场景有瓶颈
文档和社区规模远小于 GTSAM/g2o

后续：

更多 solver wrapper（GTSAM、g2o 官方支持）
多线程 Processor 扩展
更完整的视觉 SLAM 插件

7. Code Walkthrough

代码库：IRI GitLab wolf_lib

7.1 目录结构

wolf/
  include/core/
    common/node_base.h          ← 所有节点共同基类
    state_block/node_state_blocks.h  ← 含状态块的中间基类
    hardware/sensor_base.h      ← Sensor 基类
    processor/processor_base.h  ← Processor 基类
    trajectory/frame_base.h     ← Frame 基类
    factor/factor_base.h        ← Factor 基类（含残差接口）
    landmark/landmark_base.h
  src/core/                     ← 各基类实现
  plugin/                       ← 插件（vision/imu/laser/gnss/bodydynamics）
  solver/                       ← solver wrapper（目前 Ceres）

7.2 NodeBase — 所有节点基类

include/core/common/node_base.h：

身份：node_id_（静态计数器，全局唯一）、node_category_、node_type_、node_name_
problem_ptr_：weak_ptr 指回根 Problem（取用时 .lock()）
is_removing_：安全移除标志

7.3 父子链设计

无统一 "linked node" 基类；父子关系放在各节点类型内：

向上（父）：weak_ptr（如 FrameBase 持 TrajectoryBaseWPtr trajectory_ptr_）
向下（子）：shared_ptr 列表（如 FrameBase 持 CaptureBasePtrList capture_list_）
link() / remove() 负责接入/移除树
weak 向上 + shared 向下 → 免环引用，生命周期自动管理

含状态块的节点（Frame / Sensor / Landmark 等）继承 NodeStateBlocks（在 NodeBase 之上加 state block 管理）。

7.4 Factor — 约束边接口

// 派生类需实现：
virtual Eigen::VectorXd evaluateResiduals() const;
virtual JacobianMethod getJacobianMethod() const;  // ANALYTIC / AUTO / NUMERIC
virtual Eigen::MatrixXd evaluateJacobians() const;

Ceres wrapper 从 Factor 基类的这些接口自动构建 ceres::CostFunction。

7.5 Processor — 处理器接口

ProcessorBase
 ├── ProcessorMotion    ← 预积分 pipeline（IMU/里程计/接触力）
 └── ProcessorTracker   ← 特征跟踪（两种变体：跨帧 / 与 Map 地标关联）

ProcessorMotion 的核心流程直接对应 §3.2 的抽象 pipeline（pre-calibrate → delta → integrate → covariance）。

7.6 paper-vs-code 差异

论文	代码
"arbitrary solver backends"	Core 仅含 Ceres wrapper；GTSAM/g2o 为可对接选项（需实现 wrapper）
预积分"线性化 bias 修正"	`ProcessorMotion` 子类（IMU 插件）实现；标定 Jacobian 存在 `J_Delta_calib`
YAML 配置完整建问题	`ProblemLoader` 解析 YAML → 动态实例化 plugin 类（需 plugin 注册）

References

类别	链接
论文	Solà J. et al. (2022). WOLF: A Modular Estimation Framework for Robotics Based on Factor Graphs. IEEE RA-L. arXiv:2110.12919
代码	gitlab.iri.upc.edu/mobile_robotics/.../wolf
文档	mobile_robotics.pages.iri.upc-csic.es/wolf-doc-sphinx
相关	因子图优化见 SLAM 学习资源；预积分理论见 VIO 预积分

WOLF — Modular Factor Graph Estimation Framework

Table of Contents