SLAM 中的 C++ 设计模式与高级惯用法¶

难度：⭐⭐～⭐⭐⭐⭐ | 建议用时：1.5 周 | 前置要求：C++语言核心/类型转换类型转换、C++语言核心/变参模板折叠表达式与CRTP CRTP 与模板基础、C++语言核心/继承与多态深入继承与多态

前置自测¶

📋 答不出 >= 2 题时，先回顾 C++语言核心/类型转换、C++语言核心/变参模板折叠表达式与CRTP、C++语言核心/继承与多态深入的核心概念。

虚函数调用为什么比普通函数调用慢？vtable 间接寻址会阻止什么编译器优化？
static_cast<Derived*>(base) 和 dynamic_cast<Derived*>(base) 在安全性上有什么区别？
C++ 模板实例化发生在什么阶段？为什么模板函数可以被内联？
std::unique_ptr<Base> 和 std::shared_ptr<Base> 在工厂模式中分别表达什么所有权语义？
YAML 配置文件中的字符串如何映射到 C++ 的具体类型？这个过程通常叫什么模式？

本章目标¶

学完本章后，你应该能够：

面对一个 SLAM 系统，识别其中的工厂、策略、状态机、观察者、CRTP 和 Traits 模式，并解释每种模式隔离了哪个变化点
根据"变化发生在运行时还是编译时"和"调用是否在热路径"两个维度，选择运行时多态或编译期多态
为一个可配置 SLAM 前端设计工厂 + 策略 + 状态机的组合架构
实现一个配置驱动的注册表，使新增算法不需要修改中心工厂函数
设计进程内发布订阅系统，解耦 SLAM 前端和后端模块

知识树¶

SLAM 中的 C++ 设计模式与高级惯用法
├── 基础认知
│   ├── 1. 为什么 SLAM 比普通业务系统更依赖设计模式
│   └── 2. 模式选择的第一原则：变化点在哪里（运行时 vs 编译期 vs 热路径）
├── 运行时多态模式（架构层）
│   ├── 3. 工厂模式：配置驱动的对象创建（ORB-SLAM3、hdl_graph_slam、g2o）
│   ├── 4. 策略模式：可替换算法行为（OpenVINS TrackBase）
│   ├── 6. 状态机：系统运行状态管理（ORB-SLAM3 RecentlyLost）
│   └── 10. 观察者/发布订阅：模块解耦与事件分发
├── 编译期多态模式（数学层）
│   ├── 5. Traits：非侵入式数据类型适配（small_gicp、GTSAM）
│   ├── 8. CRTP：零开销抽象（Eigen MatrixBase、Sophus LieGroupBase）
│   └── 9. 类型擦除：异构容器统一存储（GTSAM 因子图）
├── 模式组合与工程实践
│   ├── 7. 综合示例：工厂 + 策略 + 状态机的组合前端
│   ├── 11. SLAM 系统的五层软件架构
│   ├── 12. 配置驱动工厂与注册表
│   ├── 13. 轻量级进程内发布订阅
│   └── 14. 模式边界：什么时候应该少用模式
└── 诊断与精读
    ├── 故障排查手册
    └── 代码精读路径

全景地图：本章围绕一个核心问题展开——SLAM 系统的变化点极多（传感器、算法、后端、地图、部署环境全都可能变化），如何把这些变化收束到可控边界内？答案是设计模式，但不是教科书式的"定义 + UML + 代码"三件套，而是从 SLAM 的具体工程问题出发，理解每种模式解决了什么问题、不用它会怎样。

本章按照"问题 → 不解决会怎样 → 模式如何解决 → 工程边界"的线索，覆盖以下知识体系：

层次	模式	要解决的 SLAM 问题	典型项目
创建层	工厂、注册表	配置文件切换算法不重编译	hdl_graph_slam、g2o
行为层	策略、模板方法、状态机	可替换算法、异常路径管理	OpenVINS、ORB-SLAM3、PCL
结构层	Facade、观察者	系统入口简化、模块解耦	KISS-ICP、GLIM
性能层	CRTP、Traits、表达式模板	热路径零开销抽象	Eigen、Sophus、small_gicp
容器层	类型擦除	异构因子统一存储	GTSAM、g2o

每个模式不是独立的知识碎片，而是"变化点管理工具箱"中的一件工具。选工具之前，先定位变化点；定位变化点之前，先理解系统中哪些东西会变、变化发生在运行时还是编译时、变化是否位于性能热路径。这三个问题构成了本章所有选型决策的共同框架。

📎 50+ 个 SLAM 项目中设计模式的频率排名统计与 SLAM 特有 C++ 模式总结，详见**附录I：SLAM 项目 C++ 技术演进与代码模式**。

1. 为什么 SLAM 比普通业务系统更依赖设计模式 ⭐¶

这一节解决什么问题：为什么在 SLAM 中谈设计模式不是”代码洁癖”，而是工程生存问题？

SLAM 系统的独特复杂性¶

Web 后端开发者讨论设计模式时，通常是为了让代码”更优雅””更可维护”。代码不够优雅，系统仍然能跑。但在 SLAM 中，设计模式是**生存问题**——如果不做正确的模块化，系统在传感器或算法发生第一次变化时就会进入不可维护的状态。

原因在于 SLAM 系统的变化点数量和类型远超普通业务系统。一个 Web 服务的变化点主要集中在业务逻辑层——数据库和前端框架通常不会频繁替换。但一个 SLAM 系统同时面临五个独立的变化维度，每个维度有多种选项，而且选项之间存在组合爆炸。一个同时支持三种传感器、四种前端算法、两种后端优化器、三种地图表达和两种部署环境的系统，理论组合数是 3 x 4 x 2 x 3 x 2 = 144 种配置。如果不用抽象边界隔离这些变化，代码中会充斥着嵌套的条件分支。

更关键的是，SLAM 是一个正在快速演进的领域。每年都有新的前端算法（从 ORB 到 SuperPoint 到事件相机）、新的后端技术（从 EKF 到图优化到 iSAM2）、新的传感器类型（固态 LiDAR、ToF 相机、4D 毫米波雷达）出现。一个 SLAM 代码库如果不能平滑地吸收新方法，很快就会被重写——而重写意味着丢失已有的调试经验和边界条件处理。

设计模式在很多业务系统里常被讲成”代码优雅性”。在 SLAM 中，它首先是生存问题。一个完整 SLAM 系统同时包含传感器输入、特征提取、匹配、优化、地图维护、回环、可视化、日志和配置管理。如果所有模块互相直接调用，系统很快会变成一个巨大的条件分支集合：

if sensor == lidar:
  if registration == icp:
    if backend == gtsam:
      ...

这种写法短期很快，长期有三个致命问题：

问题	表面现象	深层原因
难扩展	增加一种配准算法要改很多文件	创建逻辑和使用逻辑耦合
难测试	单独测试 ICP 需要启动整套系统	模块边界不清
难优化	热路径被虚函数、锁和分支打散	架构多态和数学多态混用

设计模式的价值是把变化点收束到合适的位置。SLAM 中常见变化点包括：

传感器类型会变化：单目、双目、RGB-D、LiDAR、IMU、轮速计。
前端算法会变化：KLT、ORB、ICP、GICP、NDT、直接法。
后端求解器会变化：Ceres、g2o、GTSAM、自研求解器。
地图表达会变化：稀疏点、体素、submap、TSDF、ESDF。
运行环境会变化：离线数据集、ROS2、嵌入式、云端回放。

如果不把这些变化点抽象出来，每次换算法都像做一次小型重构。设计模式的目标不是”把代码写得像教科书”，而是让变化发生在可控边界内。

这一点和硬件工程中的模块化设计是一样的。台式电脑主板定义了标准接口（PCIe、SATA、DDR 插槽），硬盘、显卡、内存可以独立替换，不需要重新设计主板。没有这些标准接口，每换一块显卡就要重新布线。SLAM 系统中的抽象接口就是这些”插槽”——Registration 接口是配准算法的插槽，FeatureTracker 接口是跟踪算法的插槽，FactorGraph 接口是优化后端的插槽。设计模式的工作就是定义这些插槽的形状。

但 SLAM 和普通业务系统有一个关键区别：不同变化点对性能的敏感度差异巨大。架构层（选择哪种传感器、哪种后端）每帧只决策一次，几纳秒的抽象开销完全不可见。数学层（每个点的残差计算、每次矩阵乘法）每帧可能执行几十万次，虚函数阻止内联的代价会累积到毫秒级。这意味着不能用同一种抽象机制处理所有变化点——架构层适合虚函数的灵活性，数学层需要模板的零开销。意识到这一点，是理解本章所有选型决策的关键。

本质洞察：设计模式在 SLAM 中不是”代码优雅性”，而是”变化点管理工具”。SLAM 系统的特殊之处在于变化点极多（传感器、算法、后端、地图、部署环境全都可能变化），而且不同变化点对性能的敏感度差异巨大——架构层每帧调用一次，数学层每帧调用几十万次。把两种变化混用同一种抽象机制，要么牺牲灵活性，要么牺牲性能。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：把所有”可能变化”的地方都做成抽象基类

错误做法：项目初期就为每个函数设计接口类、工厂类和配置项。

现象：代码结构看起来很”工程化”，但开发速度极慢，每加一个小功能都要改多个文件；而且很多接口从未有过第二个实现。

根本原因：过度抽象。设计模式是为了管理**已经存在**的变化，不是为了预防**想象中**的变化。

正确做法：先直接实现。当第二种算法或第二种传感器真正出现时，再提取接口。保持函数边界清晰，让抽象在需要时可以自然生长。

💡 概念误区：认为”设计模式 = GoF 那 23 种”

新手想法：”我要学完 23 种 GoF 模式才能写好 SLAM 代码。”

实际上：SLAM 项目中高频使用的不超过 6-8 种（工厂、策略、状态机、观察者、模板方法、外观）。更重要的 C++ 特有模式如 CRTP、Traits、表达式模板、类型擦除，在 GoF 书中根本没有。

正确理解：模式是工具，不是目标。先理解变化点，再选工具。

练习¶

[分析题] 选一个你用过的 SLAM 项目（如 ORB-SLAM3、LIO-SAM、KISS-ICP），列出它的 5 个变化点，并判断每个变化点目前是否有抽象边界。
[思考题] 为什么”if-else 链”在短期内比工厂模式更高效？从开发时间、调试难度和团队规模三个角度分析。
[设计题] 假设你正在开发一个同时支持 LiDAR 和相机的 SLAM 系统，列出至少 3 个变化点，并初步判断哪些需要运行时多态、哪些可以用编译期多态。

2. 模式选择的第一原则：变化点在哪里 ⭐⭐¶

这一节解决什么问题：面对一个需要抽象的场景，应该选择工厂、策略、模板还是 Traits？选择的依据不是”这个模式看起来更高级”，而是变化点的位置和性质。

三个判断问题¶

初学者常问：”这里该用工厂还是策略？”这个问题的方向就错了——工厂和策略解决的是不同维度的问题。更好的思路是先回答三个问题：

第一，这个地方真正会变化的是什么？ 如果变化的是”创建什么对象”——比如配置文件决定用 ICP 还是 NDT——那是工厂的领地。如果变化的是”对象的行为方式”——比如 KLT 跟踪和 ORB 跟踪的区别——那是策略的领地。工厂管”创建”，策略管”执行”。

第二，变化发生在运行时还是编译时？ 如果算法选择由用户在运行时通过配置文件决定，那必须用运行时多态（虚函数）。如果算法选择在编译时就确定——比如 small_gicp 的代价函数类型由模板参数决定——那可以用编译期多态（模板），获得内联和向量化的性能优势。

第三，变化是否位于性能热路径？ 这是 SLAM 特有的考量。一般业务系统不太关心虚函数的 2-5 ns 开销，但 SLAM 的残差计算每帧可能调用几十万次。如果变化点在热路径上（比如每个对应点的残差计算），虚函数的开销会被放大到不可接受的程度。如果变化点在冷路径上（比如每帧选择一次配准算法），虚函数的开销完全可以忽略。

这三个问题的组合决定了模式选择：

变化类型	推荐工具	SLAM 例子
运行时配置变化	Factory + virtual interface	YAML 选择 ICP/NDT/GICP
算法行为变化	Strategy	光流跟踪 vs 描述子匹配
热路径数学变化	Template / Policy	点到面 ICP 代价函数
数据类型适配	Traits	PCL/Open3D/Eigen 点云统一接口
生命周期事件	Observer / Pub/Sub	新关键帧通知后端
系统状态切换	State Machine	Tracking 初始化、正常、丢失
对外简化接口	Facade	`System::TrackMonocular()`

2.1 运行时多态与编译期多态：两种完全不同的代价模型¶

理解运行时多态和编译期多态的区别，不能只停留在"虚函数 vs 模板"的语法层面。它们的根本差异在于**决策时机**和**优化机会**。

运行时多态把"调用哪个函数"的决策推迟到程序运行时。编译器在编译调用点时不知道具体类型，只能生成一段间接调用代码——先从对象的虚表指针找到虚表，再从虚表中查找函数地址，最后跳转执行。这个间接寻址本身只需要 2-5 纳秒，对大多数场景微不足道。但真正的代价不在这几纳秒，而在**失去的优化机会**。编译器的很多优化——函数内联、常量传播、循环展开、SIMD 向量化——都依赖于"在编译期知道被调用函数的具体实现"。虚函数调用切断了这条信息链，编译器只能保守地生成通用代码。

编译期多态则把"调用哪个函数"的决策提前到编译时。模板在实例化时，编译器已经完全知道 Derived 的具体类型和所有成员函数的实现。因此，它可以把 cost.evaluate(p) 完全内联到循环体中，对循环体进行常量传播、死代码消除，甚至把多次迭代合并成 SIMD 指令。代价是：每种类型组合都会生成一份独立的机器码，增加编译时间和二进制体积。

可以用餐厅点餐来类比：运行时多态像是服务员每次都要跑到厨房问"这道菜怎么做"再回来告诉厨师（间接调用，无法提前准备）；编译期多态像是厨师在开餐前就拿到了完整菜单和每道菜的做法（完全内联，可以优化备料流程），但如果菜单有 100 道菜，每道菜都要准备一套独立的备料方案（代码膨胀）。

运行时多态的代表是虚函数：

class Registration {
public:
  virtual ~Registration() = default;
  virtual Pose align(const PointCloud& source,
                     const PointCloud& target) = 0;
};

class IcpRegistration final : public Registration {
public:
  Pose align(const PointCloud& source,
             const PointCloud& target) override {
    // ICP 实现
    return Pose{};
  }
};

它的优点是灵活：配置文件可以在运行时选择算法，一个 vector<unique_ptr<Registration>> 就能存储混合类型的对象。缺点是虚函数调用难以内联——编译器在编译 align() 的调用点时不知道具体实现在哪里，因此无法把实现代码嵌入调用点。接口也必须提前固定——基类一旦发布，增删虚函数会破坏 ABI。

编译期多态的代表是模板策略：

template <typename CostModel, typename Reduction>
class RegistrationPipeline {
public:
  Pose align(const PointCloud& source, const PointCloud& target) {
    CostModel cost;
    Reduction reduce;
    // 编译器可以内联 cost 和 reduce 的调用，适合高频数学循环。
    return solve(source, target, cost, reduce);
  }
};

它的优点是性能好：代价函数、并行归约和数据布局可以在编译期展开为具体的机器指令。编译器看到完整的计算链后，可以做循环展开、SIMD 向量化和常量传播——这些优化在虚函数调用时不可能做到。缺点是组合数量增加会带来编译时间和二进制体积压力。如果有 3 种代价函数 x 3 种并行策略 x 2 种数据布局 = 18 个模板实例，每个实例都生成独立的机器码。

工程选型可以用一句话记住：

架构层需要灵活，优先运行时多态；数学热路径需要内联，优先编译期多态。

2.2 一个反面案例：把所有东西都做成插件¶

如果把每个点的残差计算也做成虚函数插件：

for (const auto& correspondence : correspondences) {
  cost += cost_plugin->evaluate(correspondence);
}

每个对应点都会触发一次虚函数调用。对几十万点的点云配准来说，这会破坏内联和向量化。更糟的是，编译器无法看到代价函数内部结构——它不知道 evaluate() 的实现是一个简单的点积（几条指令）还是一个复杂的非线性变换（几十条指令），因此无法做常量传播、循环展开和 SIMD 向量化。

这不是说插件不好，而是**插件边界不应切入最内层循环**。这个原则可以用"海关关口"类比来理解：每次跨越抽象边界（虚函数调用）就像过一次海关——有固定的手续成本。如果你每秒只过一次海关（每帧选择一次配准算法），手续成本可以忽略；如果你每秒过十万次海关（每个对应点调用一次虚函数），手续成本就会淹没实际计算。更合理的做法是：在外层过一次海关（工厂选择算法），过了之后就进入一个"免检区"（模板策略展开的内层循环），在这个区域内没有任何间接调用。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"运行时多态 = 慢"，所以一律用模板

新手想法："虚函数有开销，我应该全部用模板来避免性能损失。"

实际上：虚函数调用开销约 2-5 ns。如果一个函数每帧只调用一次（比如选择配准算法），2 ns 完全不可见。虚函数的真正代价不是单次调用的延迟，而是它阻止编译器内联和向量化——这在每帧调用几十万次的残差计算中才真正成为问题。

正确思维：先测量调用频率。每帧一次的调度用虚函数更清晰；每点一次的计算用模板更高效。两者经常在同一个系统中共存。

⚠️ 编程陷阱：模板策略导致编译时间爆炸

错误做法：把所有策略组合都实例化：3 种代价函数 x 3 种并行策略 x 2 种数据布局 = 18 个模板实例。

现象：编译时间从 30 秒变成 5 分钟，二进制体积翻倍。

根本原因：模板实例化是编译期展开，每种组合都生成独立代码。

正确做法：只实例化实际使用的组合。用显式实例化声明（extern template）控制生成位置。或者在外层用运行时分派选择一个已编译的模板实例。

练习¶

[估算题] 一次 ICP 配准处理 10 万个对应点。如果残差函数使用虚函数，每次调用额外 3 ns，总额外开销是多少？占 10 ms 配准预算的百分比是多少？
[设计题] 给定一个支持 ICP/GICP/NDT 三种算法的点云配准模块，设计"外层运行时选择 + 内层编译期展开"的架构。画出类图或伪代码。
[跨章综合题] 回顾 C++语言核心/类型转换中 static_cast 和 dynamic_cast 的区别。在 CRTP 中为什么使用 static_cast<Derived&>(*this) 而不是 dynamic_cast？如果写错了派生类类型参数（比如 class SO3 : public LieGroupBase<SE3>），会发生什么？

3. 工厂模式：ORB-SLAM3 为什么用工厂创建传感器处理器 ⭐⭐¶

这一节解决什么问题：SLAM 系统需要支持多种传感器和多种算法，创建对象的逻辑应该放在哪里？

从一个具体问题出发¶

打开 ORB-SLAM3 的 System 构造函数，你会发现它根据 eSensor 枚举创建不同的子系统：单目、双目、RGB-D、以及各种 IMU 组合，总共六种传感器模式。每种模式需要不同的 Tracking 初始化逻辑、不同的特征匹配策略、不同的地图点创建方式。

如果不用工厂模式，最直觉的写法是在每个需要创建传感器处理器的地方都写一组条件分支。假设你的系统有三个地方需要根据传感器类型创建对象——前端初始化、数据预处理和重定位模块——那么每个地方都要维护一套完整的 if-else 链。现在想加入一种新传感器（比如事件相机），你需要找到所有这些分散的创建点，逐一添加分支。遗漏一处就是一个 bug，而且这类 bug 在编译期不会暴露，只有运行到那个代码路径时才会触发。

这就是工厂模式的动机：把”创建什么对象”的决策集中到一个地方。调用方只知道自己需要一个符合某接口的对象，工厂负责根据配置创建具体实例。这样新增传感器只需要修改工厂，不需要修改所有使用者——这就是开闭原则（Open-Closed Principle）在 SLAM 中的具体体现。

本质洞察：工厂模式的价值不是”封装了 new”，而是”把创建决策从使用者中剥离出来”。使用者只依赖抽象接口，创建决策集中在工厂。这使得新增算法的修改范围停留在工厂内部，不会向调用方扩散。

以 hdl_graph_slam 为例，select_registration_method() 从 ROS 参数读取字符串，通过 if-else 链创建 ICP、GICP、NDT 等七种配准算法之一，返回 pcl::Registration<PointT,PointT>::Ptr 基类指针。调用方只需一行代码即可通过配置文件切换算法：

工厂模式解决的是”谁负责创建对象”。在 SLAM 中，算法对象通常依赖配置：

registration:
  type: gicp
  max_iterations: 30
  voxel_size: 0.3

如果每个调用方都自己解析 type 并创建对象，配置逻辑会散落在系统各处。工厂把这个变化点集中起来：

class RegistrationFactory {
public:
  using Creator = std::function<std::unique_ptr<Registration>(const YAML::Node&)>;

  void register_creator(std::string name, Creator creator) {
    creators_.emplace(std::move(name), std::move(creator));
  }

  std::unique_ptr<Registration> create(const YAML::Node& config) const {
    const std::string type = config["type"].as<std::string>();
    auto it = creators_.find(type);
    if (it == creators_.end()) {
      throw std::runtime_error("未知配准算法: " + type);
    }
    return it->second(config);
  }

private:
  std::unordered_map<std::string, Creator> creators_;
};

这段代码的重点不是 unordered_map 的使用方式，而是它建立的**三方职责边界**。

使用者（比如 SLAM 前端）只关心 Registration 接口提供的 align() 方法。它不知道具体用的是 ICP 还是 GICP，也不关心对象是怎么创建的。这意味着前端代码完全不受算法切换的影响——你可以从 ICP 换到 NDT，前端的一行代码都不用改。

创建者（工厂类）只关心名称字符串到具体类型的映射关系。它不执行任何算法逻辑，也不知道 Registration 对象之后会被谁使用、怎么使用。创建职责被完全隔离在工厂内部。

配置文件只表达选择意图（type: gicp），不污染算法调用代码。用户改一行 YAML 就能切换算法，不需要理解 C++ 代码、不需要重编译、不需要修改任何源文件。这对实验效率至关重要——SLAM 调参时经常需要对比十几种算法配置，每次切换都重编译的话，一下午只能跑两三组实验。

工厂模式的返回类型也值得注意。这里返回 std::unique_ptr<Registration> 而不是裸指针，明确表达了"调用方拥有对象"的所有权语义。回顾 C++语言核心/预处理器与宏中智能指针的所有权规则：unique_ptr 表示独占所有权，对象生命周期由持有者控制。如果工厂返回裸指针，调用方就不知道自己应不应该 delete 它——这正是 C++ 内存错误的经典来源。

3.1 g2o 的自注册：为什么算法加载时就自动注册¶

g2o 的优化算法工厂使用了一种更精巧的机制——静态自注册。通过 G2O_REGISTER_OPTIMIZATION_ALGORITHM 宏，每个优化算法在编译为共享库后，在库被 dlopen 加载时自动向全局工厂注册自己。调用方不需要手动列出所有可用的算法——只要链接了对应的库，算法就自动可用。

这种机制的精妙之处在于它实现了真正的"开闭原则"。要新增一种优化算法（比如自研的 Dogleg 求解器），你只需要写算法实现文件、在文件末尾加一行注册宏、编译成共享库。整个 g2o 框架的代码一行都不需要改。框架只在运行时通过 OptimizationAlgorithmFactory::instance() 这个单例查找已注册的算法。

ORB-SLAM3 的 System 构造函数采用了一种更直接的方式：根据 eSensor 枚举（六种传感器模式）在构造函数内部用条件分支创建不同的子系统。这不是标准的工厂模式，但体现了同一种设计思想——传感器类型的变化被集中在 System 构造函数中，而不是散布在 Tracking、LocalMapping 和 LoopClosing 的各个角落。

g2o 等项目常用静态自注册，让算法在共享库加载时自动注册。这很方便，但有两个工程边界：

风险	原因	处理方式
静态初始化顺序	跨翻译单元初始化顺序不完全受控	使用函数内静态对象
静态库裁剪	未引用对象文件可能不被链接	使用 OBJECT 库或 whole-archive

因此，自注册适合插件生态，但要配合构建系统设计。教学项目可以先用显式注册，等模块边界稳定后再引入自注册。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：工厂函数返回裸指针

错误做法：Registration* create(const std::string& type)，调用方不知道谁负责释放。

现象：内存泄漏或 double free。

根本原因：裸指针不表达所有权。

正确做法：返回 std::unique_ptr<Registration>，表达”调用方拥有对象”。如果需要共享，返回 std::shared_ptr。

💡 概念误区：认为工厂只能用 if-else 或 map

新手想法：”工厂就是 switch-case 或 map 查表。”

实际上：工厂的核心不是查表机制，而是”创建职责的集中”。g2o 的静态自注册、ROS2 的 pluginlib、Python 的 @register_module() 装饰器，都是工厂的变体。关键是调用方不需要知道具体类型。

练习¶

[编程题] 给 RegistrationFactory 增加一个 list_available() 方法，返回所有已注册算法名称。说明它在诊断配置错误时的价值。
[分析题] hdl_graph_slam 的 select_registration_method() 使用 if-else 链。如果有 20 种算法，这个函数会怎样？提出改进方案。

4. 策略模式：OpenVINS 为什么把三种跟踪器放在同一个接口后面 ⭐⭐¶

这一节解决什么问题：SLAM 前端的主流程（接收图像 → 跟踪特征 → 估计运动）是固定的，但”怎么跟踪特征”这个步骤有多种实现方式。如何让主流程不随着跟踪算法的增减而反复修改？

问题的来源¶

OpenVINS 是一个视觉-惯性里程计系统。它的前端需要从图像中跟踪特征点，用来给后端提供视觉约束。但不同场景下最适合的跟踪方法不同：纹理丰富的室内环境适合 KLT 光流，因为光流不需要提取描述子，速度最快；纹理贫乏的工业环境可能需要 ORB 描述子匹配，因为描述子对光照变化更鲁棒；标定场景可能直接检测 Aruco 标记，因为标记提供了已知的几何约束。

如果不用策略模式，最容易想到的做法是在前端主流程中写条件分支——“如果配置选择了 KLT 就执行 KLT 代码，如果选择了 ORB 就执行 ORB 代码”。这在短期内可行，但长期来看会让主流程函数膨胀到几千行，每种算法的代码纠缠在一起，修改一种算法可能意外影响另一种的分支路径。更关键的是，单元测试一种跟踪算法时，你必须构造整个前端上下文，因为算法代码没有独立边界。

策略模式的核心思想是：把”怎么算”封装成独立对象，主流程只通过接口调用它。OpenVINS 定义了 TrackBase 纯虚基类，声明 feed_new_camera() 接口；TrackKLT、TrackDescriptor、TrackAruco 分别实现这个接口。VioManager 通过 shared_ptr<TrackBase> 调用跟踪器，完全不知道底层用的是哪种算法。这样增删跟踪器不需要改动主流程一行代码。

策略模式和工厂模式经常配合使用：工厂负责”创建哪个策略对象”，策略负责”怎么执行算法”。前端只依赖策略接口，这让系统在不修改流程代码的情况下，通过配置文件切换算法。

策略模式解决的是算法行为变化。以特征跟踪为例，同一个前端框架可能支持 KLT、ORB、Aruco 或直接法。主流程相同：

接收图像
  ↓
提取或跟踪特征
  ↓
估计相机运动
  ↓
判断是否插入关键帧

变化的是“提取或跟踪特征”的方法。策略接口可以这样设计：

class FeatureTracker {
public:
  virtual ~FeatureTracker() = default;
  virtual TrackResult track(const Image& prev,
                            const Image& curr,
                            const std::vector<Keypoint>& seeds) = 0;
};

主流程拿到 FeatureTracker 后不再关心具体实现：

class VisualFrontend {
public:
  explicit VisualFrontend(std::unique_ptr<FeatureTracker> tracker)
      : tracker_(std::move(tracker)) {}

  FrontendResult process(const Image& image) {
    if (!last_image_) {
      last_image_ = image;
      return FrontendResult{};
    }

    auto result = tracker_->track(*last_image_, image, active_points_);
    last_image_ = image;
    return estimate_motion(result);
  }

private:
  std::unique_ptr<FeatureTracker> tracker_;
  std::optional<Image> last_image_;
  std::vector<Keypoint> active_points_;
};

这里的设计让前端流程和跟踪算法分离。换 KLT 或 ORB 不需要重写 process()。注意 tracker_ 通过构造函数注入（依赖注入模式），而不是在 VisualFrontend 内部创建。这使得单元测试可以注入一个 mock 跟踪器，验证前端流程逻辑是否正确，而不需要真实的图像处理管线。

策略和工厂的配合方式也值得说明。工厂负责"根据配置创建具体策略对象"，策略负责"执行特定算法逻辑"。在系统初始化时，工厂读取 YAML 配置中的 tracker.type 字段，创建对应的 FeatureTracker 实现（可能是 TrackKLT），然后把它注入 VisualFrontend。之后的所有帧处理过程中，VisualFrontend 只通过 FeatureTracker 接口调用跟踪器，完全不知道底层用的是哪种算法。

回顾设计模式与高级惯用法前面关于变化点的讨论：策略模式管理的是"算法行为"这个变化点，工厂模式管理的是"对象创建"这个变化点。两者经常配合使用，但职责不同——工厂只管"做一个什么东西出来"，策略管"这个东西怎么工作"。

如果不用策略模式会怎样？假设直接在 process() 里写条件分支：

if (method == "klt") {
    // 100 行 KLT 代码...
} else if (method == "orb") {
    // 120 行 ORB 代码...
} else if (method == "superpoint") {
    // 150 行 SuperPoint 代码...
}

短期可行，长期会让 process() 变成几千行的巨型函数。每次修改一种算法都可能意外影响其他分支——因为所有分支共享同一个函数作用域中的局部变量。更严重的是，单元测试一种算法时必须构造整个前端对象——因为算法代码没有独立边界。你无法单独测试 KLT 跟踪器的正确性，必须把整个 VisualFrontend 搭起来，喂入真实图像，才能触发 KLT 分支。这让测试变得缓慢、脆弱且难以覆盖边界条件。

如果不用策略模式还有一个更隐蔽的长期风险：知识碎片化。当一个函数有 1000 行且包含三种算法的交错逻辑时，新加入团队的开发者很难理解每种算法的完整流程——因为它不是一个连续的代码块，而是散布在条件分支中的碎片。策略模式把每种算法封装成独立的类，每个类有自己的文件、自己的测试、自己的文档。新人想理解 KLT 跟踪器，只需要读 TrackKLT 这一个类，而不是在一个巨型函数中搜索所有 if (method == "klt") 分支。

⚠️ 常见陷阱¶

🧠 思维陷阱：策略接口设计太宽

新手想法："我要让策略接口足够通用，能适配未来所有可能的算法。"

实际上：接口越通用，参数越多，调用方要传的上下文越多。最终接口变成 virtual Result run(const Everything& ctx) = 0，失去了类型约束的价值。

正确思维：策略接口应该反映"当前已知的共同行为"。当新算法的输入输出模式不同时，应该考虑是否属于不同的抽象，而不是强行塞进同一个接口。

练习¶

[设计题] 为 SLAM 前端设计一个 KeyframePolicy 策略接口，支持"固定距离"、"固定帧数"和"视差阈值"三种关键帧选择策略。
[分析题] OpenVINS 的 TrackBase 接口有 feed_new_camera() 方法。如果你想加一种需要深度图输入的跟踪器，接口怎么改？讨论是扩展接口还是新建接口。

5. Traits：small_gicp 怎么让同一套 ICP 代码处理 PCL 和 Eigen 点云 ⭐⭐⭐¶

这一节解决什么问题：你的 ICP 算法写好了，但用户有的用 PCL 点云、有的用 Eigen 矩阵、有的用 Open3D 容器。如何让同一套算法代码适配所有这些数据结构，而不需要修改它们的源码？

问题的真实场景¶

small_gicp 是一个高性能点云配准库。它的设计目标之一是让用户可以用自己偏好的点云数据结构——PCL 用户有 pcl::PointCloud<pcl::PointXYZI>，计算几何研究者可能直接用 std::vector<Eigen::Vector3f>，Open3D 用户有自己的 open3d::geometry::PointCloud。如果 small_gicp 把 ICP 算法写成只接受 PCL 点云，Eigen 用户就得先把数据转换成 PCL 格式，这不仅增加了拷贝开销，还引入了不必要的 PCL 依赖。

最直觉的解决方案是继承——让所有点云类型继承一个公共基类。但这要求修改 PCL、Eigen、Open3D 的类定义，而这些都是第三方库，你没有权限修改。即使你有权限，不同库的维护者也不会接受为了兼容你的 ICP 而修改自己的数据结构。

Traits 模式提供了一种完全不同的思路：不修改原始类型，只在自己的代码中提供一层适配。你为每种类型写一个 Traits 特化，告诉算法"如何从这种类型中取出第 i 个点""如何获取法向量""如何查询点数"。算法只依赖 Traits 接口，不依赖具体数据结构的内存布局。

这就像给不同国家的电器配转接头——你不需要重新设计电器（修改原始类型），只需要一个转接头（Traits 特化）就能让它在不同的插座标准下工作。

SLAM 项目常要同时适配 PCL、Eigen、Open3D、自定义点云。最直接的办法是给每种类型写一套算法：

icp_for_pcl()
icp_for_eigen()
icp_for_open3d()

这会导致算法复制。Traits 的思想是把“如何访问数据”抽出来：

template <typename Cloud>
struct CloudTraits;

template <>
struct CloudTraits<std::vector<Eigen::Vector3f>> {
  static size_t size(const std::vector<Eigen::Vector3f>& cloud) {
    return cloud.size();
  }

  static Eigen::Vector3f point(const std::vector<Eigen::Vector3f>& cloud,
                               size_t i) {
    return cloud[i];
  }
};

template <typename Cloud>
Eigen::Vector3f centroid(const Cloud& cloud) {
  Eigen::Vector3f sum = Eigen::Vector3f::Zero();
  for (size_t i = 0; i < CloudTraits<Cloud>::size(cloud); ++i) {
    sum += CloudTraits<Cloud>::point(cloud, i);
  }
  return sum / static_cast<float>(CloudTraits<Cloud>::size(cloud));
}

Traits 的本质是非侵入式接口。你不需要修改 PCL 或 Open3D 的类型，只要为它们提供适配层。

为什么 Traits 比"让所有类型继承同一个基类"更好？因为你通常无法修改第三方库的类型。PCL 的 pcl::PointXYZI、Eigen 的 Vector3f、Open3D 的 PointCloud 都不是你的代码。继承要求修改类定义，Traits 只要求在你的代码中提供特化——这就是"非侵入式"的含义。

从软件设计的角度来看，Traits 模式实现了一种**静态的接口契约**。虚函数接口是运行时契约——编译器检查虚函数的签名，运行时通过虚表分派。Traits 接口是编译期契约——编译器在模板实例化时检查 Traits 特化是否提供了所需的成员函数。C++20 的 Concepts 让这种编译期契约可以用更清晰的语法表达。在 C++20 之前，如果你忘了为某种类型提供 Traits 特化，编译器会输出几百行难以理解的模板错误信息；有了 Concepts 后，错误信息会直接告诉你"类型 X 不满足 CloudConcept 约束"。

GTSAM 的 traits<Pose3> 把 Traits 模式用到了更深的层次。它通过 structure_category 标签实现编译期标签分派——lie_group_tag 标签的类型会走 Lie 群操作路径，vector_space_tag 标签的类型会走向量空间操作路径。这让 GTSAM 的优化器可以统一处理不同数学结构的变量，而不需要为每种变量类型写单独的优化代码。标签分派的好处是编译器可以在编译期就确定走哪条路径，运行时没有任何分支开销。

本质洞察：Traits 模式的价值不是"少写几行适配代码"，而是"让算法的正确性不依赖于数据容器的具体类型"。当你用 CloudTraits<T>::point(cloud, i) 替代 cloud.points[i].getVector3fMap() 时，你建立了一个抽象层：算法只依赖"能取出第 i 个点"这个能力，而不依赖 PCL 的内存布局。这让同一套 ICP 代码可以处理任何满足 Traits 约束的点云类型。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忘记为新类型提供 Traits 特化

错误做法：使用 centroid<MyCustomCloud>(cloud) 但没有写 CloudTraits<MyCustomCloud> 特化。

现象：编译错误，但错误信息极长（模板错误消息）。在 C++20 前没有 Concepts，错误信息往往不会直接告诉你"缺少 Traits 特化"。

正确做法：为每种要适配的类型写 Traits 特化，并用 static_assert 在早期检查必要接口是否存在。C++20 可以用 Concepts 约束模板参数。

练习¶

[编程题] 为 std::vector<std::array<float, 4>> 写一个 CloudTraits 特化，使前三个元素为 xyz，第四个为强度。
[思考题] small_gicp 的 Traits 同时支持 PCL 和 Eigen 点云。如果你要加入 Open3D 支持，需要改 small_gicp 的核心算法代码吗？为什么？

6. 状态机：ORB-SLAM3 为什么要发明 “RecentlyLost” 状态 ⭐⭐¶

这一节解决什么问题：SLAM 前端不是一直处于”正常跟踪”。当跟踪丢失时，系统应该怎么做？为什么 ORB-SLAM3 不是简单地从”正常”跳到”丢失”，而是增加了一个中间状态？

为什么需要状态机¶

SLAM 前端的运行状态远比”跟踪成功 / 跟踪失败”复杂。想象你正在开发一个视觉-惯性 SLAM 系统。相机突然被遮挡了 3 秒——视觉跟踪当然失败了，但 IMU 仍然在工作。如果系统一检测到视觉跟踪失败就立刻宣布”丢失”并启动代价高昂的重定位流程，那就浪费了 IMU 的预积分信息。更合理的做法是给视觉一个恢复窗口：在 3-5 秒内用 IMU 预积分维持位姿估计，同时尝试重新匹配视觉特征。如果在窗口内恢复了，就继续正常跟踪；如果超时仍未恢复，才启动完整重定位。

这就是 ORB-SLAM3 发明 RECENTLY_LOST 状态的原因。它不是一个无关紧要的优化，而是视觉-惯性融合的关键设计：利用 IMU 的短期精度为视觉恢复争取时间。没有这个中间状态，系统在短暂遮挡后的行为要么过于激进（立刻重定位，浪费计算资源），要么过于保守（等太久才重定位，导致位姿漂移不可接受）。

如果不用状态机而用布尔变量来管理这些状态，代码会迅速变得不可维护。3 个布尔变量有 8 种组合，但合法状态只有 5 种——剩下 3 种是隐含的非法状态。代码审查时几乎不可能发现所有非法组合是否被正确处理。枚举状态机把状态空间限制在 5 个合法值，编译器还可以通过 -Wswitch 检查是否遗漏了某个状态的处理。

SLAM 前端不是一直处于”正常跟踪”。它至少有这些状态：

未初始化
  ↓
初始化中
  ↓
正常跟踪
  ↓
短时丢失
  ↓
重定位
  ↓
完全丢失

如果不用状态机，代码会变成大量布尔变量：

bool initialized;
bool lost;
bool relocalizing;
bool imu_ready;
bool map_ready;

布尔变量一多，非法组合会出现：lost == true 且 initialized == false 且 relocalizing == true 到底代表什么？状态机的价值是减少非法状态。

enum class TrackingState {
  NoImagesYet,
  NotInitialized,
  Ok,
  RecentlyLost,
  Lost
};

状态转移应集中管理，而不是散落在各个函数中。每个转移都应回答：

触发条件是什么？
进入新状态时要清理哪些缓存？
哪些输出仍然可信？
是否允许插入关键帧？

这就是 ORB-SLAM3 中 Tracking 状态机值得学习的地方：它不仅控制代码路径，也表达了系统对当前估计可信度的判断。每个状态都隐含着一个对外承诺——Ok 状态意味着"当前输出的位姿可以被控制器信任"，RecentlyLost 意味着"位姿是 IMU 预积分的推测值，短期可用但精度在下降"，Lost 意味着"不要信任任何位姿输出"。控制系统需要这些信息来决定是继续执行任务还是启动安全停车。

如果不用状态机而用布尔变量，当系统需要区分"正常"、"初始化中"、"短时丢失"、"重定位中"、"完全丢失"五种状态时，你需要至少 3 个布尔变量。但 3 个布尔变量有 8 种组合，其中只有 5 种是合法的——剩下 3 种是隐含的非法状态。比如 initialized == false && relocalizing == true 是什么意思？系统都没初始化，怎么会在重定位？这种非法组合不会被编译器检查，只会在特定的执行路径中被意外触发，然后产生难以理解的行为。枚举状态机则只有 5 个值，编译器可以通过 switch 的穷举检查（-Wswitch）帮你发现遗漏的状态处理。

ORB-SLAM3 选择用枚举 + switch 而不是 GoF 风格的 State 对象，这个选择本身也值得分析。GoF State 模式为每个状态创建一个类对象，通过虚函数分派不同状态的行为。这在状态数量多（几十个）且每个状态的行为差异大的系统中有价值——比如游戏角色 AI 的状态机。但 ORB-SLAM3 只有 5 个状态，状态切换逻辑集中在 Tracking.cc 的 GrabImageXxx() 函数中，switch 语句清晰可读。引入 State 对象反而会增加内存分配（每次状态转换可能 new 一个新对象）和虚调用开销，对性能敏感的 Tracking 线程来说没有必要。

⚠️ 常见陷阱¶

🧠 思维陷阱：用 GoF State 对象替代枚举状态机

新手想法："GoF 书上说状态模式应该用 State 对象和虚函数。"

实际上：ORB-SLAM3 选择了枚举 + switch，而不是 GoF 风格的 State 对象。原因是 Tracking 代码在性能敏感路径上，状态切换频率远低于每帧处理频率。GoF State 对象增加了内存分配和虚调用开销，对于只有 5 个状态的系统没有必要。

正确思维：状态数量少（<10）且切换逻辑集中时，用枚举 + switch。状态数量多或状态行为差异大时，才考虑 State 对象。

⚠️ 编程陷阱：状态转移散落在多个函数中

错误做法：在 trackFrame()、processIMU()、handleLoopResult() 三个函数中都修改 state_。

现象：添加新状态时不确定哪些函数需要修改。状态转移条件分散，难以画出完整状态图。

正确做法：状态转移集中在一个函数或一组受控的入口中。或者至少用一个 transitionTo() 辅助函数统一处理转移日志和清理动作。

练习¶

[设计题] 为 ORB-SLAM3 的 5 种跟踪状态画状态转移图。标出每条转移的触发条件和需要执行的清理动作。
[思考题] 如果把 RecentlyLost 状态删掉，直接从 Ok 跳到 Lost，对视觉-惯性 SLAM 系统有什么影响？
[编程题] 实现一个 TrackingStateMachine 类，要求状态转移通过 transition() 方法集中管理，并在每次转移时记录日志。

7. 小型综合示例：可配置前端 ⭐⭐¶

下面把工厂、策略和状态机组合到一个简化前端里：

class Frontend {
public:
  Frontend(std::unique_ptr<FeatureTracker> tracker,
           std::unique_ptr<KeyframePolicy> keyframe_policy)
      : tracker_(std::move(tracker)),
        keyframe_policy_(std::move(keyframe_policy)) {}

  FrontendOutput process(const Image& image) {
    switch (state_) {
      case TrackingState::NoImagesYet:
        last_image_ = image;
        state_ = TrackingState::NotInitialized;
        return {};

      case TrackingState::NotInitialized:
        return initialize(image);

      case TrackingState::Ok:
        return track_normal(image);

      case TrackingState::RecentlyLost:
        return try_relocalize(image);

      case TrackingState::Lost:
        return {};
    }
    return {};
  }

private:
  FrontendOutput track_normal(const Image& image) {
    auto tracks = tracker_->track(*last_image_, image, active_points_);
    auto output = estimate_pose(tracks);

    if (!output.success) {
      state_ = TrackingState::RecentlyLost;
      return output;
    }

    if (keyframe_policy_->should_insert(output)) {
      output.create_keyframe = true;
    }

    last_image_ = image;
    return output;
  }

  std::unique_ptr<FeatureTracker> tracker_;
  std::unique_ptr<KeyframePolicy> keyframe_policy_;
  TrackingState state_ = TrackingState::NoImagesYet;
  std::optional<Image> last_image_;
  std::vector<Keypoint> active_points_;
};

这段代码体现了模式组合，值得逐条分析它的设计思想：

**策略模式**负责 FeatureTracker 和 KeyframePolicy。两个策略对象通过构造函数注入，Frontend 在整个生命周期中不知道也不需要知道具体使用的是 KLT 还是 ORB，是固定帧数还是视差阈值。这使得切换算法不需要修改 Frontend 的一行代码。

**状态机**负责跟踪状态。switch (state_) 让每种状态的处理逻辑集中且互斥——编译器可以通过 -Wswitch 检查是否遗漏了某个状态的处理。注意 RecentlyLost 状态不是简单地"等一等再重试"，而是给视觉-惯性融合系统留出用 IMU 预积分维持位姿的时间窗口。

**工厂**可以在外部根据 YAML 配置创建具体策略对象，然后注入 Frontend。Frontend 本身只表达流程逻辑，不承担任何创建职责。这种职责分离让 Frontend 在单元测试中可以接收 mock 策略对象，不需要真实的图像处理管线。

这个综合示例展示了一个重要原则：模式不是独立使用的，而是按层次组合的。工厂解决"创建什么"，策略解决"怎么执行"，状态机解决"当前处于什么状态"。三者各管一个维度的变化，互不干扰。

8. CRTP：Eigen 的 Matrix 为什么不用虚函数 ⭐⭐⭐¶

这一节解决什么问题：为什么 Eigen、Sophus、manif 等数学库选择 CRTP 而不是虚函数？这不仅仅是"性能更好"的简单回答。

从 Eigen 的设计困境出发¶

假设你要设计 Eigen 这样的矩阵库。你有 Matrix3d（固定大小 3x3 双精度矩阵）、MatrixXd（动态大小矩阵）、Map<Matrix3d>（包装外部内存的矩阵视图）、Block<MatrixXd>（矩阵的一个子块）。它们都需要支持相同的操作：加法、乘法、转置、求逆、范数等等。代码复用的需求非常明确——这些操作的数学逻辑完全相同，差异只在底层数据存储方式。

最自然的想法是继承：定义一个 MatrixBase 基类，把所有操作写在基类中。但这里有一个致命问题。考虑 operator* 的返回类型：Matrix3d * Matrix3d 应该返回 Matrix3d，MatrixXd * MatrixXd 应该返回 MatrixXd。如果基类用虚函数，返回类型只能是基类指针或引用——你无法让虚函数返回具体的派生类型。更严重的是，Matrix3d 只有 72 字节（9 个 double），它应该像 double 一样轻量、在栈上分配、可以被放进寄存器。但虚函数要求每个对象携带一个 vptr（虚表指针），这会让 72 字节变成 80 字节，破坏 SIMD 对齐，而且编译器无法跨虚调用内联——对一个每帧可能调用几十万次的数学运算来说，这完全不可接受。

CRTP 的解决方案是：在编译期就知道具体类型。基类 MatrixBase<Derived> 通过 static_cast<Derived&>(*this) 在编译期下溯到具体类型。编译器看到 MatrixBase<Matrix3d> 时，已经完全知道 Derived 就是 Matrix3d，因此所有操作都可以内联、向量化，对象不需要 vptr，返回类型可以是具体的 Matrix3d。

这就像工厂里的生产线模板：同一套装配流程（基类操作），但每条产线的零件规格在图纸阶段就确定了（编译期绑定），而不是每次装配时才临时查表决定用哪种零件（运行时多态）。

前面讲策略模式时，我们把运行时多态和编译期多态做了区分。CRTP 是编译期多态中最重要的一种写法。它的形式看起来有些反直觉：基类把派生类当作模板参数。

template <typename Derived>
class LieGroupBase {
public:
  Eigen::Matrix3d matrix() const {
    // 编译期下溯到具体类型，避免虚函数调用。
    return derived().matrix_impl();
  }

  Derived inverse() const {
    return derived().inverse_impl();
  }

private:
  const Derived& derived() const {
    return static_cast<const Derived&>(*this);
  }
};

class SO3 : public LieGroupBase<SO3> {
public:
  Eigen::Matrix3d matrix_impl() const {
    return q_.toRotationMatrix();
  }

  SO3 inverse_impl() const {
    return SO3(q_.conjugate());
  }

private:
  Eigen::Quaterniond q_;
};

如果用虚函数，也能表达同样接口：

class LieGroupRuntimeBase {
public:
  virtual ~LieGroupRuntimeBase() = default;
  virtual Eigen::Matrix3d matrix() const = 0;
  virtual std::unique_ptr<LieGroupRuntimeBase> inverse() const = 0;
};

但这个设计对 Sophus、manif、Eigen 这类数学库不合适，原因有三点：

问题	虚函数版本	CRTP 版本
内联	编译器难以跨虚调用内联	调用在编译期确定
返回类型	`inverse()` 很难返回具体派生类型	可直接返回 `Derived`
对象布局	需要 vptr，影响轻量对象	对象只保存数学数据

CRTP 的本质不是”高级语法”，而是把多态从运行时提前到编译期。对 SLAM 热路径来说，这个差异很重要。一次 SO3::exp() 或 SE3::operator*() 看似很小，但它可能在每次残差线性化、每个特征点投影、每轮优化迭代中被调用成千上万次。

让我们用一个具体的数字来感受差异。假设一次 ICP 配准处理 10 万个对应点，每个对应点需要计算一次旋转变换（调用 SO3::operator*）和一次残差计算。如果这些操作使用虚函数，每次调用额外 3 ns（vtable 间接寻址 + 阻止内联），总额外开销是 10 万 x 2 x 3 ns = 0.6 ms。这看起来不多，但 ICP 通常需要 20-30 轮迭代，累积下来就是 12-18 ms——已经接近 10 Hz 配准的帧预算。而 CRTP 在编译期确定类型后，编译器可以完全内联这些操作，甚至把多次乘法和加法合并成 SIMD 指令，额外开销为零。

Sophus 的 CRTP 实现还有一个精妙之处值得特别关注。SO3Base<Derived> 不仅服务于 SO3<double> 这种拥有内部存储的类型，还服务于 Map<SO3<double>> 这种包装外部内存指针的类型。两者共享全部 Lie 群操作代码——群乘法、逆、指数映射、对数映射——但底层数据访问方式完全不同：前者直接读自己的成员变量，后者通过外部指针间接读取。CRTP 让这两种类型在使用上完全一致，在性能上也没有任何拷贝或间接寻址开销。如果用虚函数，Map 类型的每次数据访问都要经过虚调用，这对 SLAM 中频繁使用的 Eigen::Map 来说完全不可接受。

8.1 CRTP 的代价与适用边界¶

CRTP 不是免费的性能午餐。它带来的代价需要认真评估：

错误信息的可读性。当 CRTP 代码出错时，编译器输出的错误信息往往包含多层嵌套的模板参数，长达几十行。例如，一个简单的 SO3<double> 方法签名不匹配，错误信息中可能出现 LieGroupBase<SO3Base<SO3<double, 0>>>::... 这样的层层嵌套。C++20 的 Concepts 可以显著改善这个问题——当约束不满足时，错误信息会直接指出"类型 X 不满足 LieGroupConcept"，而不是展开整个模板栈。

编译时间的增长。每种具体类型都会实例化基类的所有方法。如果基类有 30 个方法，5 种具体类型就会生成 150 个函数实例。在大型项目中（如 Eigen 库被大量使用的 SLAM 系统），这会让编译时间显著增加。

二进制体积的膨胀。模板的每个实例都生成独立的机器码。对嵌入式平台（如无人机的计算板），二进制体积可能是一个硬约束。

接口约束的隐性。虚函数接口是显式的——编译器强制要求派生类实现所有纯虚函数。CRTP 的接口是隐式的——基类通过 derived().method_impl() 调用派生类方法，如果派生类忘了实现 method_impl()，错误会在模板实例化时才暴露，而且错误信息不直观。C++20 的 Concepts 可以把这种隐式约束变成显式约束。

因此工程规则可以总结为一句话：

热路径、轻量数学对象、需要返回具体类型时用 CRTP；架构层插件和运行时算法选择仍用虚函数。

这也解释了为什么 Eigen、Sophus、manif 使用 CRTP——它们的操作在热路径中被频繁调用、对象很轻量、需要返回具体类型。而 Nav2 插件、OpenVINS tracker、g2o 优化算法工厂仍然使用运行时多态——它们的调用频率低（每帧一次）、需要运行时动态选择、需要在容器中存储混合类型。两种多态在同一个系统中共存是完全正常的，关键是把正确的工具放在正确的层次。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：CRTP 基类和派生类模板参数不匹配

错误做法：class SO3 : public LieGroupBase<SE3> {}（应该是 LieGroupBase<SO3>）。

现象：derived() 返回的 static_cast<const SE3&>(*this) 把 SO3 对象当成 SE3 读取。内存布局不同，可能读到垃圾数据或直接段错误。

根本原因：CRTP 依赖 static_cast，不做运行时类型检查。模板参数写错，编译器不会报错。

正确做法：在基类中加 static_assert 或用 C++20 Concepts 约束 Derived 必须继承自 LieGroupBase<Derived>。阅读 manif 和 Eigen 的 CRTP 实现，观察它们如何防范这类错误。

💡 概念误区：认为 CRTP 可以完全替代虚函数

新手想法："CRTP 更快，我以后全用 CRTP，不用虚函数。"

实际上：CRTP 要求在编译期知道具体类型。如果你需要一个 std::vector<std::unique_ptr<LieGroupBase>> 来存储混合类型（既有 SO3 又有 SE3），CRTP 做不到——因为 LieGroupBase<SO3> 和 LieGroupBase<SE3> 是完全不同的类型，没有公共基类。这时候仍需虚函数。

正确理解：CRTP 适合"具体类型在使用点已知"的场景（模板函数内部、算法库内部）。插件系统、容器存储、运行时选择仍需虚函数或类型擦除。

练习¶

[分析题] 比较 Sophus SO3d 和 Eigen Matrix3d 的对象大小。CRTP 是否增加了对象体积？为什么？
[编程题] 写一个最小 CRTP 示例：Shape<Derived> 基类提供 area() 方法，Circle 和 Rectangle 分别实现。验证 area() 可以被内联。
[思考题] C++20 的 Concepts 能否替代 CRTP 的部分用途？在什么场景下 Concepts 更好？在什么场景下 CRTP 仍然不可替代？

9. 类型擦除：GTSAM 的因子图为什么能同时装下 GPS 因子和 IMU 因子 ⭐⭐⭐¶

这一节解决什么问题：图优化器需要把类型完全不同的因子放进同一个容器。GPS 因子的残差是 3 维位置误差，IMU 预积分因子的残差是 15 维的，视觉重投影因子的残差是 2 维的。它们的模板参数不同、残差维度不同、雅可比结构不同，但优化器要用同一个循环遍历它们。

为什么 std::vector 装不下¶

C++ 的 std::vector 要求所有元素是同一类型。如果你定义了模板因子类，BetweenFactor<Pose3> 和 GPSFactor 是完全不同的类型——编译器会为它们生成不同的类实例，内存布局不同，成员函数地址不同。你不能把它们放进同一个 std::vector 里。

有人会想到用 std::variant——C++17 提供的类型安全联合体。但 variant 要求在定义时就列出所有可能的类型。一个真实的 SLAM 因子图可能包含几十种不同的因子类型，而且用户可能自定义新因子。variant 的类型列表会变得极长，每增加一种因子都要修改 variant 的定义。

类型擦除的思想是介于两者之间的第三条路：存储时统一用基类指针，使用时按需恢复具体类型。容器看到的是 shared_ptr<Factor> 的统一接口，每个因子内部保留了自己的具体类型信息。优化器通过虚函数 error() 和 linearize() 调用每个因子的计算逻辑，而因子内部通过 Values::at<Pose3>(key) 恢复出具体的变量类型。

这和日常生活中的快递系统很像：快递公司不关心包裹里装的是书、电器还是食品（类型擦除），统一按重量和尺寸收费和运输（基类接口）。但收件人打开包裹时，里面的东西仍然是原始类型（类型恢复）。

本质洞察：类型擦除不是"把类型信息丢掉"，而是"存储时隐藏类型，使用时恢复类型"。它比纯虚函数多一层设计——纯虚函数是"向上转型后不再关心具体类型"，类型擦除是"向上转型存储，需要时向下恢复"。GTSAM 的 NonlinearFactorGraph 用 shared_ptr<NonlinearFactor> 统一存储，但因子内部通过 Values::at<T>(key) 精确恢复出 Pose3、Point3 等具体类型，保证数学计算的类型安全。

GTSAM、g2o、Ceres 这类优化框架都要面对一个问题：因子类型千差万别。GPS 因子、IMU 预积分因子、视觉重投影因子、激光里程计因子，残差维度和测量类型都不同，但图优化器必须把它们放在同一个容器里统一调度。

最直接的想法是模板容器：

std::vector<BetweenFactor<Pose3>> factors;

这只能装一种因子。真实图优化需要：

BetweenFactor<Pose3>
GPSFactor
ImuFactor
GenericProjectionFactor<Pose3, Point3>
PriorFactor<Pose3>

类型擦除的思想是：容器只保存共同基类指针，具体类型留在对象内部。

class Factor {
public:
  virtual ~Factor() = default;
  virtual ErrorVector error(const Values& values) const = 0;
  virtual JacobianBlock linearize(const Values& values) const = 0;
};

class GpsFactor final : public Factor {
public:
  ErrorVector error(const Values& values) const override {
    const Pose3 pose = values.at<Pose3>(pose_key_);
    return pose.translation() - measured_position_;
  }
};

using FactorGraph = std::vector<std::shared_ptr<Factor>>;

这样做牺牲了部分静态类型信息，但换来了图结构的开放性。优化器不需要知道每个因子的具体 C++ 类型，只需要调用 error() 和 linearize()。

类型擦除适合”外层调度”。但因子内部的数学计算仍然可以使用模板、Eigen 表达式和自动微分，把性能留在局部。这又一次体现了本章的核心设计哲学：架构层用运行时灵活性，数学层用编译期性能。因子图的容器需要运行时灵活性（因为因子种类在编译后仍可能增加），因子内部的残差计算需要编译期性能（因为每个因子的残差和雅可比在每次迭代中被反复计算）。两个层次各用各的最优工具，通过类型擦除的边界连接起来。

⚠️ 常见陷阱¶

💡 概念误区：混淆类型擦除和虚函数继承

新手想法：”类型擦除不就是用基类指针嘛？和虚函数一样。”

实际上：类型擦除的关键区别在于”擦除后恢复”。NonlinearFactorGraph 用 shared_ptr<Factor> 统一存储，但因子内部通过 Values::at<Pose3>(key) 恢复出具体类型。虚函数继承是”向上转型后不再关心具体类型”；类型擦除是”向上转型存储，向下恢复使用”。

正确理解：类型擦除 = 存储时统一接口 + 使用时按需恢复类型信息。它比纯虚函数多一层”类型恢复”的设计。

练习¶

[分析题] GTSAM 的 NonlinearFactorGraph 如何让 GPSFactor 和 BetweenFactor<Pose3> 共存？画出它们的类层次关系。
[思考题] 如果不用类型擦除，用 std::variant<GPSFactor, BetweenFactor, ImuFactor, ...> 代替，优缺点各是什么？

10. 发布订阅与观察者：前端为什么不应该知道后端有几个模块 ⭐⭐¶

这一节解决什么问题：SLAM 前端产生关键帧后，局部建图、回环检测、可视化和日志模块都需要这个关键帧。如果前端直接调用所有后端模块，每增加一个新模块都要修改前端代码。如何让前端只负责"产出关键帧"，而不需要知道谁在消费它？

依赖反转的工程意义¶

观察者模式的本质是**依赖反转**。没有观察者模式时，前端依赖所有后端模块——前端代码里写着 local_mapper->add(kf) 和 loop_detector->add(kf)，前端必须知道后端有哪些模块。有了观察者模式后，前端只依赖一个抽象的事件总线——前端调用 bus.publish(kf)，不知道也不需要知道谁在订阅。后端模块各自向事件总线注册，按需消费事件。

这看似只是"把函数调用改成了发布订阅"，但工程影响是深远的。考虑一个场景：你的 SLAM 系统最初只有局部建图和可视化两个后端模块。几个月后，团队加入了语义分割模块、稠密建图模块、在线标定模块。如果前端直接调用后端，每加一个模块都要改前端代码、重编译、重测试。如果用了观察者模式，新模块只需要在初始化时订阅关键帧事件，前端代码完全不动。

如果不用观察者模式而直接在前端硬编码所有后端调用，还有一个更隐蔽的风险：调用顺序的耦合。前端可能无意中依赖了"先调用 local_mapper 再调用 loop_detector"这个顺序。当某天有人调换了顺序或并行化了调用，系统可能出现微妙的 bug——因为 loop_detector 依赖 local_mapper 先插入的数据。观察者模式强迫开发者思考每个消费者是否真的独立，还是有隐含的顺序依赖。

SLAM 后端常见流程是：

前端产生关键帧
  ↓
局部建图需要它
  ↓
回环检测也需要它
  ↓
可视化还需要它
  ↓
日志系统可能也要记录它

如果前端直接调用所有后续模块：

local_mapper->add_keyframe(kf);
loop_detector->add_keyframe(kf);
viewer->draw_keyframe(kf);
logger->write_keyframe(kf);

短期清楚，长期耦合严重。每增加一个模块，都要改前端代码。观察者模式把“事件产生者”和“事件消费者”分离。

class KeyframeBus {
public:
  using Callback = std::function<void(std::shared_ptr<const Keyframe>)>;

  int subscribe(Callback cb) {
    const int id = next_id_++;
    callbacks_.emplace(id, std::move(cb));
    return id;
  }

  void publish(std::shared_ptr<const Keyframe> keyframe) const {
    for (const auto& [id, cb] : callbacks_) {
      (void)id;
      cb(keyframe);
    }
  }

private:
  int next_id_ = 0;
  std::unordered_map<int, Callback> callbacks_;
};

上面的 KeyframeBus 是最简单的同步回调实现。前端调用 publish() 时，所有订阅者的回调在前端线程中顺序执行。这意味着如果可视化回调耗时 50 ms，前端就会被阻塞 50 ms——这在 10 Hz 的 SLAM 系统中是不可接受的。真实系统还要考虑：

问题	处理方式
某个订阅者很慢	独立队列或异步执行
订阅者析构	RAII 句柄自动退订
回调抛异常	捕获并隔离错误
消息很大	`shared_ptr<const T>` 只读共享
队列积压	有界队列和丢弃策略

观察者模式的本质是**依赖反转**（Dependency Inversion Principle 的具体体现）。没有观察者模式时，前端依赖后端——前端代码里写着 loop_detector->add(kf)，前端必须 #include 后端的头文件。有了观察者模式后，依赖关系反转——后端依赖前端定义的事件接口，前端不再知道后端的存在。这种反转让前端和后端可以在不同的编译单元甚至不同的库中独立演化。

从软件架构的角度看，观察者模式还实现了一种”发布-订阅解耦”：事件的生产者和消费者通过事件总线间接通信，而不是直接调用。这和 ROS 话题的设计思想完全一致——事实上，ROS 话题就是分布式的观察者模式，只不过它跨越了进程和网络边界。本节的进程内事件总线可以看作 ROS 话题的轻量级本地版本。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：回调中修改订阅列表导致迭代器失效

错误做法：在 publish() 遍历回调表的循环中，某个回调内部又调用 subscribe() 或触发退订。

现象：迭代器失效，可能崩溃或跳过某些回调。

正确做法：发布时先复制回调表快照，再遍历快照执行回调。本节的 KeyframeBus 示例采用同步调用，真实系统应考虑这个问题。

🧠 思维陷阱：过度使用观察者导致调用链不可见

新手想法：”模块之间都用事件解耦，系统更灵活。”

实际上：事件驱动会让调用链变成隐式的。直接函数调用时，IDE 可以”跳转到定义”；事件驱动时，你只知道事件被发布，不知道谁订阅了它、订阅顺序是什么、某个订阅者失败是否影响其他订阅者。

正确思维：观察者适合”一对多”且消费者可变的场景（如前端通知多个后端服务）。如果只有一个消费者且不会变化，直接调用更清晰。

练习¶

[编程题] 给 KeyframeBus 增加线程安全：发布和订阅可能从不同线程调用。
[设计题] 如果一个观察者（比如可视化模块）处理很慢，同步回调会拖慢前端。设计一种异步回调机制来解决这个问题。

11. 模式组合：一个 SLAM 系统的典型分层 ⭐⭐¶

这一节解决什么问题：单独理解每种模式不难，难的是在一个完整系统中把多种模式协调地组合在一起。本节展示 SLAM 系统的五层软件架构，以及每层对应的模式选择逻辑。

从分层的角度理解模式选择¶

前面逐个讲解了工厂、策略、状态机、观察者、CRTP、Traits 和类型擦除。但在真实 SLAM 项目中，这些模式不是独立使用的，而是分布在系统的不同层次，各自管理不同维度的变化。理解分层关系后，面对一个新模块，你应该先问"它位于哪一层"，再根据层次的特征选择合适的模式。

分层的核心依据是**抽象层次与调用频率的关系**。越上层的代码越接近用户和配置，调用频率低但变化频繁（不同用户、不同部署会切换不同算法），适合运行时多态。越底层的代码越接近数学计算，调用频率极高但变化很少（三角函数、矩阵运算的数学公式不会变），适合编译期优化。中间层负责流程编排和事件分发，既要灵活又要清晰。

把本章的模式组合起来，可以得到一个较清晰的 SLAM 软件分层：

应用层
  Facade：System / Pipeline，对外提供 Track()、SaveMap()、Reset()

编排层
  State Machine：初始化、正常、丢失、重定位
  Observer：关键帧、回环、地图更新事件

算法选择层
  Factory + Strategy：配置选择 tracker、registration、backend

数学计算层
  CRTP + Traits + Expression Template：Lie 群、点云、矩阵计算

数据容器层
  Type Erasure：不同因子、不同传感器测量统一存储

这种分层不是固定模板，而是一种判断框架。遇到新模块时，先问它位于哪一层。

**应用层**对外隐藏子系统复杂度。ORB-SLAM3 的 System 类内部有四个并行线程、一个 Atlas 多地图管理器和关键帧数据库，但对外只暴露三个 Track*() 方法。KISS-ICP 的 Pipeline 类更极致——单一 RegisterFrame() 方法封装了预处理、ICP、自适应阈值和地图更新的完整流程。这就是 Facade 模式的价值：让系统的使用者不需要理解内部结构就能正确使用系统。

**编排层**用状态机和观察者管理系统的运行流程和模块间通信。状态机确保系统在异常路径（初始化失败、跟踪丢失、重定位超时）下有确定的行为。观察者确保前端不需要知道后端有多少个消费模块。这两种模式共同让系统的"控制流"可读、可测试、可扩展。

**算法选择层**用工厂和策略管理可替换组件。这一层的关键词是"灵活性"——通过配置文件切换算法，不需要改代码。工厂负责创建，策略负责执行，两者配合让算法的增删变成局部修改。

**数学计算层**用 CRTP、Traits 和表达式模板保持零开销。这一层的关键词是"性能"——编译器必须能看到计算的全部细节，才能做内联、向量化和常量传播。任何运行时间接都会切断这条优化链。

**数据容器层**用类型擦除让不同类型的对象共存于同一容器。这一层的关键词是"开放性"——用户可以自定义新因子类型，不需要修改容器或优化器的代码。

每个模块都应该能清晰地归入某一层。如果一个模块同时承担了"算法选择"和"数学计算"两层的职责，往往说明它的抽象边界需要调整——把选择逻辑提升到上层，把计算逻辑下沉到模板。

12. 配置驱动工厂：为什么调一次参数要重编译五分钟是不可接受的 ⭐⭐¶

这一节解决什么问题：SLAM 研发中最频繁的操作之一是切换算法——今天试 ICP，明天试 GICP，后天试 NDT。如果每次切换都要改 C++ 源码、重新编译、重新部署，实验效率会极低。如何让算法切换变成改一行 YAML 配置文件的事？

从实验效率的角度理解¶

前面讲工厂模式时，用 RegistrationFactory 把创建逻辑集中起来。那个版本适合入门，但它还有一个问题：工厂函数内部仍然是 if-else 链。每增加一种算法，你需要在工厂函数中加一个分支。在多人协作的项目中，这个工厂函数很快变成冲突热点——每个人都在往同一个函数里加分支，代码评审和合并冲突的成本越来越高。

更深层的问题是"修改范围"。理想情况下，新增一种算法应该只需要做两件事：写一个算法实现类，在配置文件中填上算法名字。框架代码、主流程代码、其他算法的代码都不应该被修改。注册表模式实现了这个目标——算法在自己的 .cpp 文件中通过一行宏完成注册，主流程从配置文件读取名称，通过注册表查找并创建对象。新增算法的修改范围被限制在算法自己的文件内，不会向框架扩散。

这可以类比为学校的选课系统。传统工厂像一个人工教务员——学生说"我选微积分"，教务员翻笔记本找到对应课程并手动分配。每增开一门课，教务员的笔记本就要改。注册表像在线选课系统——每门课的老师自己在系统里注册课程信息，学生选课时只需要输入课程编号，系统自动完成匹配和分配。增开新课不需要改选课系统的代码，老师自己注册即可。

前面讲工厂模式时，用 RegistrationFactory 把创建逻辑集中起来。这个版本适合入门，但真实 SLAM 项目通常还会继续前进一层：算法不只由 C++ 代码创建，而是由配置文件创建。配置文件描述系统结构，注册表负责把字符串名称映射到具体类型。

这件事的动机很具体。调参时经常要比较 icp、gicp、ndt、vgicp，如果每换一次算法都要改 C++ 代码、重新编译、重新部署，实验效率会非常低。更麻烦的是，多人合作时每个人都可能在同一个工厂函数里加一段 else if，冲突越来越多，工厂函数也越来越像一个杂物间。

如果不做注册表，最常见的失败路径是这样的：

算法 A 写完
  ↓
修改中心工厂函数
  ↓
修改配置解析分支
  ↓
修改帮助文档中的可选项
  ↓
另一个算法 B 同时也改了同一个位置
  ↓
合并冲突，且容易漏掉某个分支

注册表把“新增算法”变成局部动作。新增一个算法类时，算法文件自己声明名称和创建函数；主流程只知道从注册表里按名称创建对象。

设计	新增算法要改哪里	风险
if-else 工厂	中心工厂函数	中心文件膨胀、合并冲突
显式注册表	启动阶段注册清单	边界清晰，但需要维护注册列表
静态自注册	算法实现文件	使用方便，但要处理链接与初始化边界

本质洞察：配置驱动工厂不是为了“少写几行 if-else”，而是为了让算法扩展的修改范围停留在算法自己的边界内。修改范围越小，实验越快，回归风险越低。

12.1 注册表的核心机制¶

注册表通常由三部分组成：

名称：配置文件中的字符串，例如 "gicp"。
创建函数：把配置节点转换成具体对象。
存储表：unordered_map<string, function<...>>。

这看起来只是一个查表，但它背后有两个重要机制。

第一，创建动作被延迟到运行时。编译器已经知道所有具体类型，但程序在读取 YAML 前不知道用户要哪一种。注册表把编译期存在的类型集合，暴露成运行时可选择的名字集合。

第二，调用方只依赖抽象接口。前端或后端拿到的是 std::unique_ptr<Registration>，它不知道对象来自 IcpRegistration 还是 NdtRegistration。这使得配置变化不会向主流程扩散。

可以把注册表类比成学校课程系统。每门课的老师知道怎么开课（算法作者写创建函数），学生选课时只填课程编号（用户在 YAML 中写算法名称），教务系统按编号找到课程并实例化（注册表按名称查找并创建对象）。学生不需要知道课程对象的构造细节，老师也不需要改学生端代码。新增一门课只需要老师注册，不需要修改选课系统——这正是注册表实现开闭原则的方式。

注册表机制在 SLAM 以外的 C++ 生态中也广泛使用。Google 的 Protocol Buffers 用类似机制注册消息类型（PROTOBUF_REGISTER_TYPE），ROS2 的 pluginlib 用 PLUGINLIB_EXPORT_CLASS 宏注册插件类，OpenCV 的算法工厂也遵循类似的模式。学会一种注册表实现，就能理解所有这些框架的插件机制。

#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <unordered_map>

class Registration {
public:
  virtual ~Registration() = default;
  virtual Pose align(const PointCloud& source,
                     const PointCloud& target) = 0;
};

class RegistrationRegistry {
public:
  using Creator =
      std::function<std::unique_ptr<Registration>(const YAML::Node&)>;

  static RegistrationRegistry& instance() {
    // 函数内静态对象在第一次调用时初始化，可避免跨文件初始化顺序问题。
    static RegistrationRegistry registry;
    return registry;
  }

  void add(std::string name, Creator creator) {
    // 同名注册通常代表配置名称冲突，启动阶段直接暴露更容易定位。
    const bool inserted =
        creators_.emplace(std::move(name), std::move(creator)).second;
    if (!inserted) {
      throw std::runtime_error("重复注册配准算法名称");
    }
  }

  std::unique_ptr<Registration> create(const YAML::Node& config) const {
    // type 字段是配置驱动工厂的入口，缺失时应给出明确错误。
    const std::string type = config["type"].as<std::string>();
    auto it = creators_.find(type);
    if (it == creators_.end()) {
      throw std::runtime_error("未知配准算法: " + type);
    }
    return it->second(config);
  }

private:
  std::unordered_map<std::string, Creator> creators_;
};

这个版本使用 instance() 提供全局注册表。它不是因为全局变量优雅，而是因为插件创建本身需要一个全局命名空间：同一个算法名称在整个进程中只能有一个含义。

12.2 静态注册宏的好处与边界¶

很多 C++ 项目会把注册动作包装成宏：

#define REGISTER_REGISTRATION(NAME, TYPE)                                      \
  namespace {                                                                  \
  const bool registered_##TYPE = [] {                                          \
    RegistrationRegistry::instance().add(                                      \
        NAME, [](const YAML::Node& config) {                                   \
          /* 具体类型只在自己的文件里出现，主流程不依赖它。 */                 \
          return std::make_unique<TYPE>(config);                               \
        });                                                                    \
    return true;                                                               \
  }();                                                                         \
  }

class GicpRegistration final : public Registration {
public:
  explicit GicpRegistration(const YAML::Node& config) {
    // 中文注释应解释配置项的工程含义，而不是复述语法。
    max_iterations_ = config["max_iterations"].as<int>(30);
    voxel_size_ = config["voxel_size"].as<double>(0.3);
  }

  Pose align(const PointCloud& source,
             const PointCloud& target) override {
    // 这里省略真实 GICP 迭代，重点是展示创建边界。
    return run_gicp(source, target, max_iterations_, voxel_size_);
  }

private:
  int max_iterations_ = 30;
  double voxel_size_ = 0.3;
};

REGISTER_REGISTRATION("gicp", GicpRegistration)

宏的好处是新增算法时不需要修改中心注册清单。但它有清晰边界：

边界	失败现象	根本原因	工程处理
静态库裁剪	配置写了名称却找不到算法	对象文件未被链接进最终程序	用显式引用、OBJECT 库或整库链接
初始化顺序	注册表还没初始化就写入	跨翻译单元初始化顺序不稳定	使用函数内静态对象
名称冲突	后注册算法覆盖前者	缺少冲突检测	重复名称直接报错
配置错误	构造时抛出模糊异常	缺少字段校验	在创建函数中检查字段并带上名称

这里容易误解的一点是：静态自注册不是插件系统的全部。它只解决”名字如何进入注册表”。真正的插件还需要动态库加载、版本检查、ABI 边界和错误隔离。教学代码可以先用显式注册，因为显式注册更容易调试——注册清单就在代码里，调试时可以断点。当算法数量增加、模块边界稳定后，再引入静态自注册。

静态自注册的核心原理是利用 C++ 的**静态初始化**机制。在文件作用域声明的 static const bool 变量，其初始化表达式会在程序启动（或共享库加载）时自动执行。宏展开后，初始化表达式调用 Registry::instance().add(name, creator)，把算法注册到全局注册表。这个过程发生在 main() 之前，因此当 main() 开始运行时，所有已链接的算法都已经在注册表中了。

但这个机制有一个隐蔽的陷阱：静态库的死代码裁剪。如果注册宏所在的 .cpp 文件编译为静态库（.a），且主程序没有直接引用该文件中的任何符号，链接器可能认为这个对象文件”没有被使用”而不链接它。结果是算法没有注册，配置文件写了名称却找不到算法，运行时报错。解决方案包括使用 CMake 的 OBJECT 库（不经过链接器裁剪）、使用 --whole-archive 链接选项（强制链接所有对象文件）、或在主程序中添加一个显式引用。这类问题在小规模项目中很难遇到，但当项目拆分成多个子库时几乎必定出现。

12.3 配置驱动的反面失败¶

配置驱动工厂也会被误用。最典型的误用是把配置文件变成“第二套编程语言”。例如：

frontend:
  type: visual
  if_low_texture_use: klt
  if_high_speed_use: imu_only
  if_loop_detected_reset_tracker: true
  special_case_17: enable

这类配置看似灵活，实际会让系统行为难以推理。配置应该描述结构和参数，不应该承载复杂业务逻辑。复杂逻辑应该回到 C++ 状态机或策略对象中，并配套测试。

判断配置边界可以问三个问题：

问题	适合放配置	应回到代码
是否只是选择一个算法实现	是	否
是否只是给数值参数赋值	是	否
是否包含多步状态转移规则	否	是
是否影响线程生命周期	通常否	是
是否需要单元测试覆盖复杂分支	通常否	是

如果不守住这个边界，系统会出现一种很隐蔽的失败：同一份二进制在不同配置下表现完全不同，但行为差异没有经过编译器和单元测试保护。排查时看 C++ 代码找不到原因，最后才发现配置文件里某个开关改变了状态转移。这类问题的调试成本极高，因为它不在代码审查的覆盖范围内——代码审查关注的是代码变更，而问题出在配置文件的变更。

Python 生态中 MMEngine（OpenMMLab 基础框架）的 MODELS.build(cfg) 是配置驱动工厂的成熟实现。它使用 @MODELS.register_module() 装饰器注册模块，_module_dict 中心查找表存储映射，cfg.pop('type') 提取类型名并查找构造。C++ 版本用宏和静态初始化替代了 Python 的装饰器和 import 触发机制，但核心思想完全一致：算法实现自己负责注册，框架只负责按名称查找和创建。

12.4 小练习¶

将第 3 节的 RegistrationFactory 改写为 RegistrationRegistry，要求重复名称注册时报错，并在错误信息中包含算法名称。
设计一份 YAML，使同一个前端可以选择 klt 或 orb 跟踪策略，同时选择 distance 或 view_angle 关键帧策略。
思考静态自注册和显式注册哪一种更适合嵌入式部署。要求从链接可控性、启动错误定位和二进制体积三个角度回答。

13. 轻量级进程内发布订阅：为什么核心算法库不应该依赖 ROS ⭐⭐¶

这一节解决什么问题：你写了一个 SLAM 核心算法库，它需要在单元测试、离线数据集评测、嵌入式设备和 ROS2 节点中同时使用。如果核心库直接用 ROS 话题做模块间通信，它就被绑死在 ROS 环境中——单元测试需要启动 ROS 运行时，嵌入式设备可能根本没有 ROS。如何在不依赖 ROS 的前提下实现模块间的事件分发？

核心库与系统集成层的分离¶

现代 SLAM 工程的最佳实践是"纯 C++ 核心库 + 薄系统封装"。核心库只包含算法逻辑，不依赖 ROS、不依赖 Qt、不依赖任何系统框架。ROS2 节点只是核心库的一个薄封装，负责消息转换、参数传递和生命周期管理。这种分离让核心库可以在任何环境中复用——包括用 GoogleTest 写的单元测试、用 Python 调用的离线评测脚本、甚至移植到没有 ROS 的嵌入式平台。

但核心库内部仍然需要模块间通信。前端产出关键帧后，回环检测模块、局部建图模块和日志模块都需要收到通知。如果不用发布订阅而让前端直接调用这些模块，核心库内部的耦合度就会升高（上一节已经讨论过这个问题）。解决方案是在核心库内部实现一个轻量级的进程内事件总线——它不需要网络、不需要序列化、不需要 DDS，只需要在同一个进程内通过 shared_ptr<const T> 零拷贝传递事件。

ROS 的话题机制很强，但并不是所有 SLAM 代码都应该依赖 ROS。核心算法库常常需要在单元测试、离线评测、嵌入式程序和 ROS 节点中同时使用。如果核心库直接发布 ROS 消息，它就很难在普通 C++ 测试中运行。

进程内发布订阅解决的是另一个层次的问题：同一个进程中，模块之间怎样传递事件而不互相认识。它不负责网络通信，也不负责跨进程发现，只负责把“新关键帧”“地图更新”“回环成功”这类事件分发给感兴趣的模块。

它和 ROS 话题的关系可以类比为函数调用和远程调用。进程内事件总线更轻、更快、更容易测试，但隔离性较弱；ROS 话题更重，却能跨进程、跨机器，并且有成熟的可视化与录包工具。

维度	进程内发布订阅	ROS 话题
数据传递	指针或共享对象	消息序列化与 QoS
主要目标	解耦模块	系统集成
测试成本	普通单元测试即可	需要节点和执行器
失败边界	同进程异常影响更直接	进程隔离更好

本质洞察：发布订阅不是让调用链消失，而是把调用链从“写死在生产者代码里”变成“由订阅关系在系统装配时形成”。这能降低耦合，但也要求事件语义足够清楚。

13.1 Topic 的最小可用设计¶

一个可用的进程内话题至少需要三件事：

订阅：模块能注册回调。
发布：生产者能发送消息。
退订：订阅者析构后不会留下悬空回调。

下面的示例用 RAII 句柄管理退订。为了保持教学重点，示例采用同步发布；真实系统可以在此基础上加入每个订阅者的独立队列。

#include <functional>
#include <memory>
#include <mutex>
#include <unordered_map>

class Subscription {
public:
  Subscription() = default;
  explicit Subscription(std::function<void()> unsubscribe)
      : unsubscribe_(std::move(unsubscribe)) {}

  ~Subscription() {
    if (unsubscribe_) {
      // 析构时自动退订，避免回调指向已经释放的对象。
      unsubscribe_();
    }
  }

  Subscription(const Subscription&) = delete;
  Subscription& operator=(const Subscription&) = delete;

  Subscription(Subscription&& other) noexcept
      : unsubscribe_(std::move(other.unsubscribe_)) {
    other.unsubscribe_ = nullptr;
  }

private:
  std::function<void()> unsubscribe_;
};

template <typename T>
class Topic {
public:
  using MessagePtr = std::shared_ptr<const T>;
  using Callback = std::function<void(MessagePtr)>;

  Subscription subscribe(Callback callback) {
    std::lock_guard<std::mutex> lock(mutex_);
    const int id = next_id_++;
    callbacks_.emplace(id, std::move(callback));
    return Subscription([this, id] {
      // 退订也要加锁，保证不会和发布过程同时修改回调表。
      std::lock_guard<std::mutex> lock(mutex_);
      callbacks_.erase(id);
    });
  }

  void publish(MessagePtr message) {
    std::unordered_map<int, Callback> snapshot;
    {
      std::lock_guard<std::mutex> lock(mutex_);
      // 复制回调表快照，避免回调内部再次订阅或退订造成迭代器失效。
      snapshot = callbacks_;
    }
    for (const auto& [id, callback] : snapshot) {
      (void)id;
      callback(message);
    }
  }

private:
  std::mutex mutex_;
  int next_id_ = 0;
  std::unordered_map<int, Callback> callbacks_;
};

这段代码有一个重要选择：发布时先复制回调表，再执行回调。这样做牺牲了一点拷贝开销，但避免了一个常见死锁：回调内部又调用 subscribe() 或触发其他事件，如果发布函数一直持有锁，就会形成自锁或锁顺序反转。

这个设计决策体现了一个更普遍的工程原则：不要在持有锁的时候调用外部代码。回调函数是"外部代码"——你不知道用户会在回调里做什么。如果用户在回调中又发布了另一个事件，或者又调用了 subscribe，而发布函数一直持有 mutex，就会形成死锁。先复制回调表、释放锁、再执行回调，虽然多了一次 map 拷贝，但消除了死锁风险。在多线程 SLAM 系统中，这类看似"多此一举"的防御性设计往往是系统能否稳定运行几小时的关键。

13.2 事件语义比接口更重要¶

发布订阅最容易出问题的地方不是代码，而是事件语义不清。例如 MapUpdated 到底表示：

地图点数量改变了？
关键帧位姿被全局校正了？
地图版本号增加了？
只是可视化需要刷新？

如果一个事件名称承载太多含义，订阅者会做出不一致假设。更稳妥的做法是按语义拆分事件：

事件	含义	典型订阅者
`NewKeyframe`	新关键帧已经冻结，可只读使用	局部建图、回环检测
`LoopAccepted`	回环约束已经通过验证	后端优化、日志
`MapSnapshotReady`	一致地图快照已生成	可视化、保存模块
`TrackingLost`	当前前端输出不再可信	状态监控、重定位

如果不拆分事件，反面失败会很明显：可视化把 MapUpdated 当成“重画地图”，后端把它当成“可以开始优化”，日志把它当成“保存关键帧”。某次只更新了点云颜色，却意外触发重优化，系统延迟突然升高。

13.3 同步回调、异步队列和背压¶

进程内事件总线有三种常见执行方式：

方式	行为	适用场景	边界
同步回调	发布者线程直接执行回调	轻量通知、测试	慢订阅者会拖慢发布者
每订阅者队列	发布只入队，订阅者自己处理	可视化、日志	要设计队列容量
保留最新值	只保留最近一次消息	状态显示、UI	中间事件会丢失

关键帧进入建图通常不能随意丢——每个关键帧都携带了视觉约束信息，丢失意味着地图中留下空洞。可视化地图可以丢旧帧——用户看到的永远是最新状态就够了，中间过渡帧没有价值。状态显示只需要最新值——比如当前位姿估计、当前跟踪状态，订阅者只关心"现在是什么"而不是"中间经历了什么"。

这些策略不应该藏在回调内部，而应该在订阅时由订阅者明确声明。否则一个慢日志模块就可能拖慢 Tracking（因为同步回调会阻塞发布者），或者一个慢可视化窗口让地图更新排队数百帧（因为队列无界增长）。回顾多线程架构与混合开发中关于队列背压的讨论——进程内事件总线同样需要背压控制，否则"解耦"只是表面现象，延迟压力仍然通过队列堆积从消费者传导回生产者。

13.4 小练习¶

给 Topic<T> 增加异常隔离：某个回调抛出异常时记录错误，但不影响后续回调执行。
设计 LatestTopic<T>：发布时覆盖旧值，订阅者读取时只拿最新消息。说明它为什么适合位姿显示，不适合关键帧建图。
将多线程架构与混合开发的有界队列思想接入本节事件总线：为每个订阅者设置独立队列，并定义队列满时的策略。

14. 模式边界：什么时候应该少用模式 ⭐⭐⭐¶

这一节解决什么问题：设计模式不是越多越好。本节讨论过度抽象的危害、识别信号和退场策略。

为什么”多抽一层”有时候是错的¶

设计模式的危险之处在于，它很容易让人觉得”多抽一层总是更专业”。在 SLAM 中，这个想法会直接伤害性能和可调试性。考虑一个具体场景：你正在开发一个研究原型，系统只有一种传感器（LiDAR）、一种配准算法（ICP）、一种地图表达（体素地图）。如果此时就搭建完整的工厂 + 策略 + 注册表 + 观察者框架，每加一个小功能都要在四五个文件中协调修改。代码结构看起来很”工程化”，但开发速度极慢，调试时也很难跟踪一个调用链穿过多少层抽象。

模式的目标是管理**已经存在**的变化，不是为了预防**想象中**的变化。如果你的系统目前只有一种配准算法，未来三个月也不打算加第二种，那为 ICP 设计一个抽象基类和工厂就是过度设计。保持函数边界清晰、命名规范、接口明确就够了。等第二种算法真正出现时，再从具体代码中提取接口也不迟——这时候你已经有两个具体实现来指导接口设计，设计出来的接口会比凭想象预测出来的更准确。

这和 YAGNI（You Aren't Gonna Need It）原则是同一个道理。设计模式是后验工具（在变化出现后使用），不是先验预测工具（在变化出现前预防）。

一个实用判断是：先估计变化频率，再估计调用频率。

调用频率	变化频率	推荐做法
低	高	工厂、策略、插件
高	低	直接函数、模板、内联
高	高	外层可配置，内层编译期策略
低	低	保持简单，不急着抽象

这个表能解释很多工程选择。配准算法整体每帧调用一次，算法类型经常变化，因此适合工厂和策略；点到面的残差在一次配准中可能调用几十万次，公式不常变，因此适合模板或普通内联函数。

14.1 反面案例：抽象穿透热路径¶

考虑一个点云配准残差：

class ResidualModel {
public:
  virtual ~ResidualModel() = default;
  virtual double evaluate(const Point& source,
                          const Point& target) const = 0;
};

double accumulate_cost(const std::vector<Correspondence>& pairs,
                       const ResidualModel& residual) {
  double cost = 0.0;
  for (const auto& pair : pairs) {
    // 每个对应点都触发一次虚调用，编译器难以内联残差公式。
    cost += residual.evaluate(pair.source, pair.target);
  }
  return cost;
}

这个设计看起来很灵活，但它把虚函数放进了最内层循环。后果包括：

后果	机制
无法内联	编译器在编译 `accumulate_cost()` 时不知道具体残差类型
SIMD 受限	残差公式对优化器不可见
分支预测压力	大量间接调用增加前端开销
调试误导	性能问题看起来像算法慢，其实是抽象边界错了

更合理的做法是把“选择残差模型”放在外层，把“计算残差”放在模板内层：

struct PointToPlaneResidual {
  double operator()(const Point& source, const Point& target) const {
    // 点到面距离：目标法向量与位置差的点积。
    const Eigen::Vector3d diff = source.position - target.position;
    return target.normal.dot(diff);
  }
};

template <typename Residual>
double accumulate_cost_fast(const std::vector<Correspondence>& pairs,
                            const Residual& residual) {
  double cost = 0.0;
  for (const auto& pair : pairs) {
    // 残差类型在编译期已知，编译器可以内联并继续优化循环。
    const double r = residual(pair.source, pair.target);
    cost += r * r;
  }
  return cost;
}

这不是否定运行时多态，而是把它放在合适的位置。外层可以用工厂选择 point_to_plane 或 gicp，选定后进入具体模板函数，内层循环就不再付虚调用成本。这种"外层运行时分派 + 内层编译期展开"的组合，是 SLAM 中最常见的架构模式。small_gicp 就是这样设计的：用户在配置文件中选择代价函数类型和并行策略，系统在启动时通过工厂选择对应的模板实例，运行时每帧只执行已经完全展开的模板代码。

如果不这样分层会怎样？把虚函数一路推到最内层循环，编译器无法看到残差公式的内部结构，无法做常量传播（比如知道法向量是单位向量）、循环展开（知道残差维度固定为 3）和 SIMD 向量化（相邻对应点的计算可以并行）。最终表现是：代码"看起来很灵活"——任何人都可以通过继承加入新残差——但实际性能可能比手写的简单函数慢 5-10 倍。灵活性和性能的权衡不是在整个系统层面做一次选择，而是在每个抽象边界处做局部决策。

14.2 抽象泄漏的三个信号¶

当一个模式边界设计错了，代码会发出信号：

信号	说明	调整方向
接口参数越来越多	抽象没有抓住共同点，只是在透传细节	拆分接口或回到具体类型
使用者频繁向下转型	基类接口不足，调用方仍想知道具体类型	重新设计接口边界
性能分析显示大量间接调用	抽象进入热路径	外层多态，内层模板

“抽象泄漏”不是说抽象失败，而是说抽象层没有封住它应该隐藏的变化。比如 Registration 接口如果暴露了 set_voxel_leaf_size()、set_ndt_resolution()、set_gicp_epsilon()，那调用方其实已经知道了所有具体算法细节，这个基类就失去了意义。

14.3 模式退场也是设计能力¶

有时最好的设计是删除模式。例如一个研究原型只有一种传感器、一种配准算法、一种地图表达，此时强行搭建完整插件框架只会降低调试速度。等第二种算法真的出现，再抽出接口也不迟。

这和建筑工程中的脚手架很像：脚手架在施工期间必不可少，但建筑完工后如果不拆除脚手架，建筑反而不好用。设计模式中的抽象层就是"软件脚手架"——它在系统需要灵活性时提供支撑，但当灵活性不再需要时，它就变成了增加理解成本和调试难度的累赘。一个只有一种实现的接口，一个从未有过第二个算法的工厂，一个只在初始化时设置一次的策略——这些都是"已完工后仍未拆除的脚手架"。

工程成熟度不仅体现在"何时引入模式"，更体现在"何时移除模式"。能准确判断一个抽象层是否仍然在提供价值，需要对系统的变化历史和未来方向有清晰的认知。

可以用下面的决策流程判断是否引入模式：

这个变化点现在是否真实存在？
  ├─ 否 → 保持直接实现，留下清晰函数边界
  └─ 是
      ↓
变化是否发生在运行时？
  ├─ 是 → 考虑工厂、策略、状态机
  └─ 否
      ↓
调用是否位于热路径？
  ├─ 是 → 考虑模板、Traits、CRTP
  └─ 否 → 普通类和函数通常足够

这个流程的核心是克制。设计模式不是提前猜测所有未来变化，而是在变化已经有形状时，把它收束到稳定边界。

14.4 小练习¶

找出你写过的一段 SLAM 或机器人代码，标注其中“真实变化点”和“想象出来的变化点”。说明哪些抽象可以暂时删除。
将本节虚函数残差示例改成运行时选择外层、模板执行内层的版本，要求配置仍能选择残差类型。
对一个因子图优化器做边界分析：哪些地方适合类型擦除，哪些地方应保留模板或直接函数？

前面介绍了何时引入模式、何时移除模式。接下来通过精读真实项目代码来巩固这些判断，然后用故障排查手册将所有模式选型错误的症状汇总成可查阅的参考表。

16. 代码精读路径 ⭐⭐¶

建议按下面顺序阅读真实项目：

hdl_graph_slam registrations.cpp
看配置字符串如何映射到 ICP/GICP/NDT 等配准对象。这是最直观的工厂模式。
OpenVINS tracker 层
看 TrackBase 如何抽象不同特征跟踪策略。重点观察接口如何保持稳定。
ORB-SLAM3 Tracking.cc
看状态机如何控制初始化、正常跟踪、短时丢失和重定位。重点不是复制代码，而是理解状态转移条件。
small_gicp traits 与 registration 模板
看 Traits 和 policy-based design 如何适配不同点云类型与并行策略。
Sophus 或 manif 的 CRTP 基类
看基类如何复用 Lie 群操作，同时让 SO3、SE3 和 Map 类型保持零开销抽象。

阅读时不要只看”用了什么模式”，而要问：”这个模式隔离了哪个变化点？如果不用它，代码会在哪里变坏？”这个问题比”这是哪种 GoF 模式”重要得多——前者帮你理解设计决策，后者只是分类标签。

阅读顺序也很重要。建议先读 KISS-ICP 和 hdl_graph_slam 这类结构简单的项目，建立对工厂和策略的直觉；再读 ORB-SLAM3 观察状态机和多线程交互；最后读 Eigen 和 Sophus 的 CRTP 实现。如果一上来就读 Eigen 的模板代码，很容易被复杂的编译期元编程劝退，但如果先理解了”为什么需要编译期多态”，再看 Eigen 的实现就会觉得每一步都有清晰的工程理由。

16A. 状态机模式在机器人行为树中的深入应用 ⭐⭐⭐¶

这一节解决什么问题：前面讨论了 ORB-SLAM3 中基于枚举的简单状态机。但现代机器人系统——尤其是 Nav2 导航栈——使用的是更加复杂的**行为树（Behavior Tree, BT）**架构。行为树本质上是状态机模式的泛化，它引入了层次化组合、异步执行和黑板通信等机制，解决了传统有限状态机在复杂任务编排中面临的组合爆炸问题。

从有限状态机到行为树的演进¶

前面第 6 节中介绍的 ORB-SLAM3 状态机只有五种状态和固定的转移图。这在 SLAM 跟踪管理中已经足够——状态数量有限、转移条件清晰、每种状态的行为差异明确。但当你从 SLAM 转向导航和操作任务时，状态数量和转移条件会急剧增长。

考虑一个移动机械臂的自主搬运任务：机器人需要导航到目标位置、识别物体、规划抓取路径、执行抓取、确认抓取成功、导航到放置位置、放置物体。每个阶段都可能失败，失败后需要不同的恢复策略——导航失败可以重规划、抓取失败可以重试或换抓取姿态、识别失败可以调整相机角度。如果用传统有限状态机表达这个任务，状态数量很快超过 20 个，状态转移图变成一张难以阅读的蜘蛛网。更致命的是，新增一种恢复策略意味着在转移图中添加多条新边，影响多个已有状态。

行为树的核心思想是把复杂行为分解为**可组合的节点树**。每个叶子节点执行一个原子动作（如”移动到位置 A”或”闭合夹爪”），内部节点控制子节点的执行逻辑（顺序执行、并行执行、条件分支、重试）。通过组合这些基本构件，可以表达任意复杂的行为，同时保持每个组件的简单性和可测试性。

特征	有限状态机（FSM）	行为树（BT）
结构	状态 + 转移图	树形节点层次
扩展性	新增状态需修改转移图	新增子树不影响父节点
复用性	状态通常不可复用	子树可以在不同任务中复用
可读性	状态少时清晰，状态多时混乱	层次结构天然具有可读性
典型应用	SLAM 跟踪状态、低层控制	导航任务、抓取操作、自主决策

Nav2 行为树的实际架构¶

Nav2 的导航任务编排完全基于 BehaviorTree.CPP 库。导航过程不是一个硬编码的函数调用链，而是一棵 XML 描述的行为树。每个 BT 节点对应一个 C++ 类，继承自 BT::ActionNodeBase、BT::ConditionNode 或 BT::DecoratorNode。

<!-- Nav2 默认导航行为树（简化版） -->
<root main_tree_to_execute=”MainTree”>
  <BehaviorTree ID=”MainTree”>
    <PipelineSequence>
      <RateController hz=”1.0”>
        <ComputePathToPose goal=”{goal}” path=”{path}”/>
      </RateController>
      <FollowPath path=”{path}”/>
    </PipelineSequence>
  </BehaviorTree>
</root>

这棵树的执行语义是：PipelineSequence 按顺序执行子节点；RateController 限制 ComputePathToPose 的调用频率为 1 Hz；路径规划的结果通过黑板变量 {path} 传递给 FollowPath。如果 FollowPath 返回 RUNNING（正在执行），PipelineSequence 会在下一个 tick 中同时检查规划和跟随——这实现了”边走边重规划”的行为。

从设计模式的角度看，BT 节点就是策略模式的一种组织方式。每个 Action 节点封装了一种可替换的行为实现，BT 的 XML 配置文件扮演了工厂的角色——它决定了哪些节点被实例化、按什么顺序执行。BT 工厂通过 BT::BehaviorTreeFactory::registerNodeType<MyNode>(“MyNode”) 注册节点类型，XML 通过名称引用——这和前面讨论的配置驱动注册表是完全一致的机制。

编写自定义 BT 节点¶

以一个自定义的”检查电池电量”条件节点为例：

#include <behaviortree_cpp/condition_node.h>

// 条件节点：检查电池电量是否足够继续任务
class BatteryCheck : public BT::ConditionNode {
public:
  BatteryCheck(const std::string& name,
               const BT::NodeConfig& config)
      : BT::ConditionNode(name, config) {}

  static BT::PortsList providedPorts() {
    // 输入端口从黑板读取电池百分比，阈值可配置
    return {BT::InputPort<double>(“battery_level”),
            BT::InputPort<double>(“min_level”, 20.0, “最低电量阈值”)};
  }

  BT::NodeStatus tick() override {
    double battery = 0.0;
    double threshold = 20.0;
    getInput(“battery_level”, battery);
    getInput(“min_level”, threshold);
    // SUCCESS 表示条件满足（电量足够），FAILURE 表示条件不满足
    return (battery >= threshold)
               ? BT::NodeStatus::SUCCESS
               : BT::NodeStatus::FAILURE;
  }
};

注册节点后就可以在 XML 中引用它：

BT::BehaviorTreeFactory factory;
factory.registerNodeType<BatteryCheck>(“BatteryCheck”);
// 通过 XML 配置行为树
auto tree = factory.createTreeFromFile(“navigation_bt.xml”);

这个设计和 pluginlib 的注册机制在结构上完全同构——都是”名称 → 类型”的映射。Nav2 的 BT 节点通过 pluginlib 动态加载，这意味着你可以在不重编译 Nav2 的前提下添加新的行为节点。

FSM 与 BT 的边界选择¶

并非所有场景都应该用行为树替代状态机。行为树适合任务层的行为编排，但底层控制和实时状态管理仍然适合传统状态机。

场景	推荐方案	原因
SLAM 跟踪状态管理	枚举状态机	状态少（5 个）、转移条件简单、实时性要求高
导航任务编排	行为树	步骤多、需要恢复策略、任务可复用
机械臂操作序列	行为树	步骤可组合、失败处理复杂
电机控制器状态	状态机	安全关键、转移条件严格

本质洞察：状态机和行为树不是替代关系，而是互补关系。状态机管理的是系统的**运行模式**（正常/丢失/重定位），行为树管理的是任务的**执行流程**（导航→抓取→放置）。一个完整的机器人系统通常两者都需要——行为树的叶子节点内部可能包含状态机。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：在 BT 节点中执行阻塞操作

错误做法：在 tick() 中调用 std::this_thread::sleep_for() 或等待 ROS service 同步返回。

现象：整个行为树被阻塞，其他节点无法 tick，看门狗超时。

根本原因：BT 的 tick 函数应该是非阻塞的。长时间操作应该在第一次 tick 时发起请求、返回 RUNNING，后续 tick 检查完成状态。

正确做法：使用 BT::StatefulActionNode 的 onStart() / onRunning() / onHalted() 三阶段模型。

🧠 思维陷阱：认为行为树可以完全替代状态机

新手想法：”行为树更先进，我应该全部用行为树。”

实际上：行为树在任务层优势明显，但在实时控制层引入了不必要的复杂性。一个简单的”使能/禁用/错误”三态控制器用枚举状态机三行代码就能表达，换成行为树反而需要配置 XML、注册节点、管理黑板。

正确思维：任务层（秒级决策）用行为树，控制层（毫秒级决策）用状态机。

练习¶

[设计题] 为一个四足机器人设计行为树：巡逻 → 发现障碍 → 避障 → 继续巡逻。失败时重试三次后回到起点。画出 BT 节点树并标注每个节点的类型。
[编程题] 用 BehaviorTree.CPP 库实现一个 IsGoalReached 条件节点，从黑板读取当前位姿和目标位姿，判断欧氏距离是否小于阈值。
[分析题] Nav2 的 navigate_to_pose_w_replanning_and_recovery.xml 包含重规划和恢复行为。解释 RecoveryNode 和 RoundRobin 组合节点在其中的作用。

16B. C++20/23 对设计模式的影响 ⭐⭐⭐¶

这一节解决什么问题：前面介绍的设计模式大多基于 C++11/14/17 的语言能力。C++20 引入了 Concepts、协程和模块，C++23 引入了 std::expected 和 std::print。这些新特性正在改变一些经典模式的实现方式，某些情况下甚至让模式变得不再必要。

Concepts 对策略模式和 CRTP 的影响¶

C++20 的 Concepts 是对 SFINAE 和 CRTP 约束的一次根本性改进。在 C++17 及之前，模板策略参数没有显式约束——如果你传入一个不符合要求的类型，得到的是一长串难以阅读的模板实例化错误。Concepts 让你可以在编译期用声明式语法表达”这个模板参数必须满足什么条件”。

考虑 small_gicp 风格的模板策略。C++17 时代，代价函数的约束是隐式的——文档说”代价函数必须提供 operator()(source, target) 方法”，但编译器不检查：

// C++17：代价函数的约束是隐式的，不满足时的错误信息极差
template <typename CostFunction>
double accumulate_cost(const PointCloud& src,
                       const PointCloud& tgt,
                       CostFunction& cost) {
  double total = 0.0;
  for (size_t i = 0; i < src.size(); ++i) {
    // 如果 CostFunction 没有 operator()，这里会产生几百行错误
    total += cost(src[i], tgt[i]);
  }
  return total;
}

C++20 的 Concepts 把隐式约束变成显式声明：

// C++20：代价函数的约束是显式的，不满足时的错误信息清晰
template <typename T>
concept PointCostFunction = requires(T cost,
                                     const Point& src,
                                     const Point& tgt) {
  { cost(src, tgt) } -> std::convertible_to<double>;
};

template <PointCostFunction CostFunction>
double accumulate_cost(const PointCloud& src,
                       const PointCloud& tgt,
                       CostFunction& cost) {
  double total = 0.0;
  for (size_t i = 0; i < src.size(); ++i) {
    total += cost(src[i], tgt[i]);
  }
  return total;
}

如果传入一个不满足 PointCostFunction concept 的类型，编译器会给出清晰的错误：”类型 X 不满足 PointCostFunction 约束，因为 cost(src, tgt) 表达式不合法。”这比 C++17 的几百行模板展开错误有了质的飞跃。

Concepts 对 CRTP 的影响更加深远。回顾第 8 节中 CRTP 基类的一个经典问题——派生类模板参数写错时编译器不报错。C++20 可以用 Concepts 约束 CRTP 的派生类型：

// 约束：Derived 必须继承自 LieGroupBase<Derived>
template <typename Derived>
concept IsLieGroupDerived =
    std::derived_from<Derived, LieGroupBase<Derived>>;

template <IsLieGroupDerived Derived>
class LieGroupBase {
protected:
  const Derived& derived() const {
    return static_cast<const Derived&>(*this);
  }
};

// ✅ 正确：SO3 继承自 LieGroupBase<SO3>
class SO3 : public LieGroupBase<SO3> {};

// ❌ 编译错误：SE3 传给了 LieGroupBase<SO3>，不满足约束
// class SE3 : public LieGroupBase<SO3> {};  // 编译器会清晰报错

但 Concepts 不会让 CRTP 消亡。CRTP 的核心价值不只是编译期约束——它还提供了基类复用具体实现的能力（derived() 访问派生类成员）。Concepts 解决了约束问题，但基类代码复用仍然需要 CRTP 或 C++23 的 deducing this。

`std::expected` 对错误处理策略的影响¶

C++23 的 std::expected<T, E> 提供了一种替代异常的类型安全错误传播方式。在 SLAM 和机器人控制中，异常有两个实际问题：性能开销（异常路径不可预测的栈展开）和实时性冲突（硬实时系统通常禁用异常）。

传统的工厂模式在创建失败时通常抛异常：

// C++17：创建失败时抛异常
std::unique_ptr<Registration> create(const YAML::Node& config) {
  auto it = creators_.find(type);
  if (it == creators_.end()) {
    throw std::runtime_error(“未知配准算法: “ + type);
  }
  return it->second(config);
}

C++23 可以用 std::expected 替代异常，让错误成为返回值类型的一部分：

// C++23：创建失败通过返回值传播，不使用异常
std::expected<std::unique_ptr<Registration>, RegistrationError>
create(const YAML::Node& config) {
  auto it = creators_.find(type);
  if (it == creators_.end()) {
    return std::unexpected(RegistrationError::UnknownType);
  }
  return it->second(config);
}

// 调用方必须处理错误，编译器强制检查
auto result = factory.create(config);
if (!result) {
  log_error(result.error());
  return;
}
auto registration = std::move(*result);

std::expected 的优势是错误处理变成了类型系统的一部分——调用方不可能”忘记”处理错误，因为 expected 的值只有在检查成功后才能访问。这在安全关键的机器人系统中尤为重要。

PIMPL 模式与 ABI 稳定性工程实践¶

PIMPL（Pointer to Implementation）不是 GoF 的 23 种模式之一，但在 C++ 机器人库中极其重要。它的核心思想是把类的私有成员隐藏到一个前向声明的实现类中，公共头文件只暴露指向该实现类的指针。

PIMPL 解决两个工程问题。第一，编译防火墙——修改实现细节（增删私有成员、修改内部数据结构）不需要重编译所有依赖该头文件的代码。在大型机器人项目中，一个核心头文件可能被上百个翻译单元包含，每次修改触发的级联重编译可能耗时数分钟。PIMPL 把重编译范围限制在实现文件内部。

第二，更关键的是 ABI 稳定性。当你以共享库（.so）形式分发 SLAM 算法库时，下游用户编译的代码依赖你的头文件中的类布局（对象大小、成员偏移）。如果你在新版本中给类添加了一个私有成员变量，类的内存布局改变了，所有下游代码必须重新编译——否则访问成员的偏移量不对，程序会崩溃或产生未定义行为。PIMPL 让公共头文件中的类永远只有一个 unique_ptr<Impl> 成员，对象大小固定为一个指针，添加实现细节不改变 ABI。

// slam_core.hpp — 公共头文件，ABI 稳定
#include <memory>
#include <Eigen/Core>

class SlamCore {
public:
  SlamCore();
  ~SlamCore();

  // 公共 API 使用 Eigen 类型但不暴露内部实现
  Eigen::Isometry3d track(const PointCloud& cloud);
  void reset();

  // 禁止拷贝，允许移动
  SlamCore(SlamCore&&) noexcept;
  SlamCore& operator=(SlamCore&&) noexcept;
  SlamCore(const SlamCore&) = delete;

private:
  struct Impl;                   // 前向声明，下游无需知道内部结构
  std::unique_ptr<Impl> impl_;   // 对象大小固定为一个指针
};

// slam_core.cpp — 实现文件，可以自由修改
struct SlamCore::Impl {
  // 这些成员的增删不影响 ABI
  VoxelMap map_;
  IcpSolver solver_;
  TrackingState state_ = TrackingState::NotInitialized;
  int frame_count_ = 0;
  // 新版本可以添加新成员，下游不需要重编译
};

SlamCore::SlamCore() : impl_(std::make_unique<Impl>()) {}
SlamCore::~SlamCore() = default;  // 必须在 cpp 中定义，因为 Impl 在 hpp 中不完整

ROS2 的 rclcpp::Node 内部就大量使用 PIMPL——这是 ROS2 能在不破坏二进制兼容性的前提下频繁更新的重要原因。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：PIMPL 的析构函数定义在头文件中

错误做法：在头文件中使用 ~SlamCore() = default;。

现象：编译错误——Impl 是不完整类型，编译器无法生成 unique_ptr<Impl> 的析构函数。

根本原因：unique_ptr 的析构函数需要知道被管理类型的完整定义才能调用 delete。前向声明只告诉编译器”有这个类型”，但不告诉它类型的大小和析构函数。

正确做法：在 .cpp 文件中定义析构函数——SlamCore::~SlamCore() = default;。移动构造和移动赋值同理。

💡 概念误区：认为 Concepts 可以完全替代虚函数接口

新手想法：”Concepts 能约束类型，以后策略模式全用 Concepts 就好了。”

实际上：Concepts 约束的是**编译期**类型——你必须在编译时知道具体类型。运行时从配置文件选择算法的场景，仍然需要虚函数或类型擦除。Concepts 替代的是 SFINAE 和隐式模板约束，不是运行时多态。

正确理解：Concepts 让编译期多态的约束更清晰，但运行时多态的适用场景不变。

练习¶

[编程题] 为第 5 节的 Traits 系统添加 C++20 Concepts 约束。定义一个 PointCloudConcept，要求类型提供 size()、operator[] 和返回 Eigen::Vector3f 的坐标访问。
[设计题] 将本章的 RegistrationRegistry::create() 改为返回 std::expected<std::unique_ptr<Registration>, CreateError>。讨论这种方式与异常方式在实时控制场景下的权衡。
[分析题] 阅读 KISS-ICP 的公共 API 头文件（Pipeline.hpp）。它是否使用了 PIMPL？如果没有，分析添加 PIMPL 的利弊。

16C. 观察者模式与 ROS2 话题的同构性 ⭐⭐¶

这一节解决什么问题：前面第 10 节和第 13 节分别介绍了观察者模式和进程内发布订阅。本节把视角拉高一层，分析观察者模式与 ROS2 话题之间的结构同构性，以及这种同构性如何指导核心算法库与 ROS2 集成层的设计。

结构同构：从进程内到跨进程¶

进程内观察者和 ROS2 话题在结构上是同构的——它们都实现了”发布-订阅”的通信范式，区别仅在于通信的范围和机制。

维度	进程内观察者	ROS2 话题
注册	`bus.subscribe(callback)`	`node->create_subscription(topic, qos, callback)`
发布	`bus.publish(msg)`	`publisher->publish(msg)`
数据传递	`shared_ptr<const T>` 零拷贝	DDS 序列化/反序列化（进程内可零拷贝）
发现机制	编译时已知	运行时 DDS 发现
QoS	无（由应用层自行管理）	可靠性、历史深度、生命周期等
工具支持	无标准工具	`ros2 topic echo`、`rosbag2`、RViz

认识到这种同构性，核心算法库的事件系统就可以设计成与 ROS2 话题一一对应的接口。当核心库运行在 ROS2 环境中时，ROS2 wrapper 把每个内部事件桥接到一个 ROS2 话题；当核心库运行在单元测试或嵌入式环境中时，内部事件直接通过进程内总线分发。桥接层的代码极其简单：

// ROS2 wrapper：把核心库的内部事件桥接到 ROS2 话题
class SlamNode : public rclcpp::Node {
  SlamCore core_;
  rclcpp::Publisher<PoseStamped>::SharedPtr pose_pub_;

  void setup_bridges() {
    // 核心库产生位姿事件时，转换为 ROS2 消息并发布
    core_.on_pose_updated([this](const Eigen::Isometry3d& pose) {
      auto msg = to_ros_msg(pose);
      pose_pub_->publish(msg);
    });
  }
};

这种桥接设计让核心库保持对 ROS2 的零依赖，同时享受 ROS2 生态的全部工具——录包、回放、可视化、多机通信。回顾 CMake从入门到工程化中关于核心库与 ROS2 wrapper 分离的讨论，事件桥接正是连接两者的关键机制。

MoveIt2 插件系统的设计模式¶

MoveIt2 是 ROS2 生态中设计模式运用最密集的项目之一。它的插件系统完全基于 pluginlib——ROS2 的动态加载框架。pluginlib 本质上是一个运行时工厂，通过 XML 清单文件注册插件类，运行时按名称加载共享库并实例化对象。

MoveIt2 中插件化的组件包括：

组件	基类接口	典型插件
运动规划器	`PlannerManager`	OMPL、Pilz、CHOMP、STOMP
IK 求解器	`KinematicsBase`	KDL、TRAC-IK、IKFast、pick_ik
碰撞检测	`CollisionDetectorAllocator`	FCL、Bullet
约束采样	`ConstraintSamplerManager`	默认采样器、自定义采样器

以运动规划器为例，pluginlib 的注册和加载流程如下：

// 在 OMPL 插件的 .cpp 文件末尾注册
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(
    ompl_interface::OMPLPlannerManager,
    planning_interface::PlannerManager)

<!-- 插件清单 XML（由 ament 安装到包资源目录） -->
<library path=”libmoveit_ompl_planner_plugin”>
  <class name=”ompl_interface/OMPLPlanner”
         type=”ompl_interface::OMPLPlannerManager”
         base_class_type=”planning_interface::PlannerManager”>
    <description>OMPL motion planning plugin for MoveIt2</description>
  </class>
</library>

运行时，MoveIt2 的 PlanningPipeline 通过 pluginlib 的 ClassLoader 按名称加载规划器。用户在 moveit_config 的 YAML 中指定规划器名称，完全不需要修改 MoveIt2 的源码。这和第 12 节的配置驱动注册表是同一种设计思想，只不过 pluginlib 多了动态库加载的能力——插件甚至不需要在编译时链接到主程序。

PCL 点类型注册的 Traits 模式¶

PCL（Point Cloud Library）的点类型系统是 Traits 模式在机器人库中的另一个典型应用。PCL 需要支持多种点类型——PointXYZ（仅坐标）、PointXYZI（坐标 + 强度）、PointXYZRGB（坐标 + 颜色）、PointNormal（坐标 + 法向量）——每种类型的内存布局不同。PCL 的算法（滤波、配准、分割）需要在不同点类型上工作，但不应该为每种类型写一份独立的实现。

PCL 使用 POINT_CLOUD_REGISTER_POINT_STRUCT 宏注册自定义点类型的字段信息：

struct CustomPoint {
  PCL_ADD_POINT4D;     // 添加 x, y, z, padding
  float intensity;
  float timestamp;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

POINT_CLOUD_REGISTER_POINT_STRUCT(
    CustomPoint,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (float, timestamp, timestamp))

注册后，PCL 的所有模板算法——pcl::VoxelGrid<CustomPoint>、pcl::PassThrough<CustomPoint>、pcl::IterativeClosestPoint<CustomPoint, CustomPoint>——都能自动适配这个自定义类型。PCL 内部通过 Traits 查询每种点类型的字段列表和内存偏移，算法实现中使用 pcl::getFieldsList<PointT>() 获取字段信息。

这和第 5 节的 small_gicp Traits 系统是同一种思想，但 PCL 的实现更加依赖宏和模板元编程——它需要在编译期生成字段访问代码，以便后续的序列化、反序列化和字段映射操作。

练习¶

[设计题] 为一个 LiDAR SLAM 核心库设计事件接口（on_pose_updated、on_map_updated、on_tracking_lost），然后编写 ROS2 wrapper 的桥接代码。要求核心库不包含任何 ROS2 头文件。
[分析题] 阅读 MoveIt2 的 PlanningPipeline 源码，找出它通过 pluginlib 加载规划器的代码路径。解释 pluginlib 和本章注册表的设计差异。
[编程题] 定义一个自定义 PCL 点类型 PointXYZIT（坐标 + 强度 + 时间戳），注册后用 pcl::VoxelGrid 滤波。

17. 练习与思考 ⭐⭐¶

为上面的 Frontend 增加一个 RelocalizationStrategy，说明它应该用运行时多态还是编译期多态。
设计一个 CloudTraits 特化，使同一个 centroid() 函数能处理 PCL 点云和 std::vector<Eigen::Vector3f>。
解释为什么点云配准最内层残差计算不适合用虚函数插件。
画出你熟悉的一个 SLAM 项目的变化点清单，并标注每个变化点适合工厂、策略、Traits、状态机还是发布订阅。
选择一个已有 SLAM 项目，找出一个 Facade 类、一个 Strategy 接口、一个 Factory 函数或注册表，并说明它们如何协作。

🔧 故障排查手册¶

症状	可能的设计问题	排查步骤	修复方向
增加一种配准算法要改 5 个以上文件	缺少工厂或策略边界	1. 画出算法创建的调用链 2. 找到所有 `if (type == "xxx")` 3. 检查是否有统一接口	抽出接口，创建逻辑集中到工厂
点云配准内层循环性能差	热路径使用运行时多态	1. 用 perf 查看虚函数调用占比 2. 检查残差计算是否跨虚调用 3. 对比内联版本耗时	外层工厂选择算法，内层改成模板策略
系统状态混乱，偶发不合法行为	用多个布尔变量替代状态机	1. 列出所有状态相关变量 2. 枚举合法组合 3. 检查非法组合是否被处理	改为枚举状态 + 集中转移函数
新增模块需要修改前端代码	前端直接调用所有后端模块	1. 画出前端对后端的调用关系 2. 检查是否有事件分发机制	引入观察者或消息总线
模板编译错误信息长达数百行	CRTP 使用过度或约束不足	1. 检查 CRTP 基类的 `static_cast` 是否类型正确 2. 添加 `static_assert`	在架构层换回虚接口，或加 Concepts 约束
下游类型适配困难	算法直接操作具体数据结构	1. 检查算法函数签名中的具体类型 2. 是否可替换为 Traits 接口	用 Traits 做非侵入适配
配置驱动的算法找不到注册名	静态自注册在链接时被裁剪	1. 检查注册代码所在的 `.cpp` 是否被链接 2. 搜索 CMake 是否用了 OBJECT 库	使用 `--whole-archive` 或显式引用

19. 跨章综合练习 ⭐⭐⭐¶

[C++语言核心/类型转换 + 设计模式与高级惯用法] 回顾 C++语言核心/类型转换中 static_cast 的安全边界。CRTP 中的 static_cast<Derived&>(*this) 如果 Derived 类型写错会触发未定义行为。设计一个编译期检查机制（使用 static_assert 或 C++20 Concepts），使这类错误在编译时被发现。
[C++语言核心/变参模板折叠表达式与CRTP + 设计模式与高级惯用法] 回顾 C++语言核心/变参模板折叠表达式与CRTP 中模板特化的机制。为 small_gicp 风格的 Traits 系统增加一个 static_assert 检查：如果某个类型没有提供 Traits 特化，在使用时给出清晰的错误信息而不是几百行模板展开错误。
[多线程架构与混合开发 + 设计模式与高级惯用法] 回顾下一章的多线程架构。将本章的观察者模式（KeyframeBus）改为线程安全版本，使 publish() 和 subscribe() 可以从不同线程调用。要求不使用全局锁阻塞发布者。

20. 本章小结¶

SLAM 中的设计模式不是为了追求形式，而是为了管理变化。传感器会变、前端会变、后端会变、地图表示会变、部署环境会变；如果变化点没有被隔离，系统会在规模增长后变得难以维护。

本章的核心判断可以压缩成三条工程规则：

规则一：架构层的可替换算法用工厂和策略。 工厂管创建（"用哪种算法"），策略管执行（"算法怎么工作"）。两者配合让算法切换变成改一行 YAML 配置的事，不需要改 C++ 代码、不需要重编译。hdl_graph_slam 的 select_registration_method()、OpenVINS 的 TrackBase 接口、g2o 的 OptimizationAlgorithmFactory 都是这条规则的体现。

规则二：系统运行状态用状态机表达，不要靠零散布尔变量。 ORB-SLAM3 的 RecentlyLost 状态不是可有可无的标签——它给视觉-惯性融合系统提供了用 IMU 维持位姿的时间窗口。状态机的价值在于约束合法状态空间、集中管理状态转移逻辑、让编译器帮你检查遗漏。

规则三：数学热路径用 CRTP、Traits 和模板策略保留编译期优化。 Eigen 的 MatrixBase<Derived> 不用虚函数是因为矩阵乘法每帧调用几十万次，虚函数的间接寻址会阻止内联和 SIMD 向量化。small_gicp 用模板策略组合代价函数和并行策略，编译器可以把代价函数完全内联到归约循环中，这是它比 fast_gicp 快约 2 倍的核心原因。

三条规则的共同底层逻辑是**变化点分层**：架构层的变化用运行时灵活性应对，数学层的变化用编译期性能应对，中间层的流程和事件用状态机和观察者组织。理解了这个分层框架，所有模式选择都不再是死记硬背，而是对"变化点在哪里、调用频率多高、性能敏感吗"这三个问题的自然回答。

模式	管理的变化点	典型位置	选型信号
工厂	对象创建方式	配置驱动的算法选择	算法类型由配置决定
策略	算法行为	特征跟踪、关键帧策略	同一流程不同实现
状态机	系统运行状态	Tracking 初始化/正常/丢失	多个状态，每个状态行为不同
观察者	事件消费者	关键帧通知后端	生产者不应知道消费者
CRTP	编译期多态	Lie 群、矩阵运算	热路径、轻量对象
Traits	数据类型适配	PCL/Eigen/Open3D 统一接口	不能修改原始类型
类型擦除	异构容器	因子图、传感器混存	不同模板类型统一存储
Facade	系统复杂度	`System::Track()`	对外隐藏子系统细节

真正成熟的 SLAM 代码往往是多种模式的组合：外层用 Facade 提供简单入口，中层用状态机和观察者组织流程，算法层用工厂和策略管理可替换组件，数学层用模板和 Traits 保持性能。理解这些分层后，再读 ORB-SLAM3、OpenVINS、GTSAM、small_gicp 或 Sophus，就不会只看到零散 C++ 技巧，而能看到它们背后的工程结构。

最后值得强调的一个观念是：设计模式是手段，变化点管理是目的。SLAM 系统的独特之处在于它同时面对三种截然不同的变化压力——算法层面的变化（研究新方法）、工程层面的变化（适配新传感器、新平台）和性能层面的变化（不同硬件上的优化策略不同）。能否为每种变化找到合适的抽象边界，决定了系统能否从"一个人的研究原型"成长为"团队维护的工程产品"。本章介绍的每种模式，都是在这个成长过程中被反复验证的工具。

累积项目：本章新增模块¶

**SLAM 工程化实践项目**续：

本章新增模块：可配置前端架构

实现 FeatureTracker 策略接口和至少两种具体实现（如 TrackKLT、TrackDescriptor）
实现 RegistrationRegistry，支持从 YAML 配置文件按名称创建配准算法
实现 TrackingStateMachine，包含 NoImagesYet、NotInitialized、Ok、RecentlyLost、Lost 五种状态
实现进程内 Topic<T> 事件总线，支持关键帧发布和多模块订阅
用 Frontend 综合类把以上模块组合起来，通过配置文件选择算法组合

延伸阅读¶

资源	难度	说明
Design Patterns: Elements of Reusable Object-Oriented Software (GoF, 1994)	⭐⭐	经典 23 种模式的原始定义，但需要结合 SLAM 场景重新理解适用边界
Modern C++ Design (Alexandrescu, 2001)	⭐⭐⭐⭐	策略类、类型列表、小对象分配器等 C++ 模板元编程的奠基之作
ORB-SLAM3 源码 `Tracking.cc`	⭐⭐	状态机、传感器分派和多地图管理的真实实现
OpenVINS 源码 `TrackBase` / `TrackKLT`	⭐⭐	策略模式在视觉-惯性里程计中的典型应用
small_gicp 源码	⭐⭐⭐	Traits + 模板策略在高性能点云配准中的最佳实践
Sophus 源码 `SO3Base` / `SE3Base`	⭐⭐⭐	CRTP 在 Lie 群库中的零开销抽象实现
GTSAM 源码 `NonlinearFactorGraph`	⭐⭐⭐	类型擦除在因子图优化中的工程应用

SLAM 中的 C++ 设计模式与高级惯用法¶

前置自测¶

本章目标¶

知识树¶

1. 为什么 SLAM 比普通业务系统更依赖设计模式 ⭐¶

SLAM 系统的独特复杂性¶

⚠️ 常见陷阱¶

练习¶

2. 模式选择的第一原则：变化点在哪里 ⭐⭐¶

三个判断问题¶

2.1 运行时多态与编译期多态：两种完全不同的代价模型¶

2.2 一个反面案例：把所有东西都做成插件¶

⚠️ 常见陷阱¶

练习¶

3. 工厂模式：ORB-SLAM3 为什么用工厂创建传感器处理器 ⭐⭐¶

从一个具体问题出发¶

3.1 g2o 的自注册：为什么算法加载时就自动注册¶

⚠️ 常见陷阱¶

练习¶

4. 策略模式：OpenVINS 为什么把三种跟踪器放在同一个接口后面 ⭐⭐¶

问题的来源¶

⚠️ 常见陷阱¶

练习¶

5. Traits：small_gicp 怎么让同一套 ICP 代码处理 PCL 和 Eigen 点云 ⭐⭐⭐¶

问题的真实场景¶

⚠️ 常见陷阱¶

练习¶

6. 状态机：ORB-SLAM3 为什么要发明 “RecentlyLost” 状态 ⭐⭐¶

为什么需要状态机¶

⚠️ 常见陷阱¶

练习¶

7. 小型综合示例：可配置前端 ⭐⭐¶

8. CRTP：Eigen 的 Matrix 为什么不用虚函数 ⭐⭐⭐¶

从 Eigen 的设计困境出发¶

8.1 CRTP 的代价与适用边界¶

⚠️ 常见陷阱¶

练习¶

9. 类型擦除：GTSAM 的因子图为什么能同时装下 GPS 因子和 IMU 因子 ⭐⭐⭐¶

为什么 std::vector 装不下¶

⚠️ 常见陷阱¶

练习¶

10. 发布订阅与观察者：前端为什么不应该知道后端有几个模块 ⭐⭐¶

依赖反转的工程意义¶

⚠️ 常见陷阱¶

练习¶

11. 模式组合：一个 SLAM 系统的典型分层 ⭐⭐¶

从分层的角度理解模式选择¶

12. 配置驱动工厂：为什么调一次参数要重编译五分钟是不可接受的 ⭐⭐¶

从实验效率的角度理解¶

12.1 注册表的核心机制¶

12.2 静态注册宏的好处与边界¶

12.3 配置驱动的反面失败¶

12.4 小练习¶

13. 轻量级进程内发布订阅：为什么核心算法库不应该依赖 ROS ⭐⭐¶

核心库与系统集成层的分离¶

13.1 Topic 的最小可用设计¶

13.2 事件语义比接口更重要¶

13.3 同步回调、异步队列和背压¶

13.4 小练习¶

14. 模式边界：什么时候应该少用模式 ⭐⭐⭐¶

为什么”多抽一层”有时候是错的¶

14.1 反面案例：抽象穿透热路径¶

14.2 抽象泄漏的三个信号¶

14.3 模式退场也是设计能力¶

14.4 小练习¶

16. 代码精读路径 ⭐⭐¶

16A. 状态机模式在机器人行为树中的深入应用 ⭐⭐⭐¶

从有限状态机到行为树的演进¶

Nav2 行为树的实际架构¶

编写自定义 BT 节点¶

FSM 与 BT 的边界选择¶

⚠️ 常见陷阱¶

练习¶

16B. C++20/23 对设计模式的影响 ⭐⭐⭐¶

Concepts 对策略模式和 CRTP 的影响¶

std::expected 对错误处理策略的影响¶

PIMPL 模式与 ABI 稳定性工程实践¶

⚠️ 常见陷阱¶

练习¶

16C. 观察者模式与 ROS2 话题的同构性 ⭐⭐¶

`std::expected` 对错误处理策略的影响¶