代码质量、测试与调试¶

难度：⭐⭐～⭐⭐⭐⭐ | 建议用时：2 周 | 前置要求：C++语言核心/RAII与智能指针 RAII、C++语言核心/错误处理与异常安全错误处理、并发与系统编程/线程管理与互斥同步线程同步、CMake从入门到工程化 CMake 测试集成

定位：本章讨论机器人 C++ 项目的质量防线。

前面的章节已经讲过类型系统、RAII、移动语义、模板、并发、内存模型、缓存和 CUDA。这些能力让程序能写得更快、更抽象、更接近硬件。但能力越强，错误的形态也越隐蔽。

本章解决的问题是：

如何在 SLAM、感知和控制系统中，用测试、静态分析、Sanitizer、调试器和性能分析工具，把错误尽早暴露出来。

本章学习目标¶

学完本章后，你应该能做到：

为几何、优化、并发和 IO 模块设计有意义的测试。
区分单元测试、集成测试、回归测试、属性测试和数据集测试。
正确使用 GoogleTest 写浮点、矩阵、李群和异常测试。
用 Google Benchmark 做可信的性能测量，而不是测到编译器优化后的幻觉。
使用 ASan、UBSan、TSan 发现内存越界、未定义行为和数据竞争。
用 GDB/LLDB 定位崩溃、死锁、异常路径和多线程状态。
用 clang-tidy、编译器警告和格式化工具维持代码质量。
用 perf、Callgrind、火焰图和 trace 找到真正的热点。
设计适合机器人项目的 CI 门禁。
将一次 bug 复盘转化为长期有效的测试用例。

本章的核心思想是：

测试不是为了证明程序完全正确。

测试是为了让错误在代价最低的位置暴露。

知识树¶

代码质量、测试与调试
├── 为什么机器人项目需要质量工具（33.1）
│   └── 分层防线、不变量思维
├── 测试金字塔（33.2）
│   ├── 静态检查 → 单元 → 组件 → 集成 → 数据集
│   └── 越底层越快、越精准
├── GoogleTest（33.3）
│   ├── 浮点比较、旋转矩阵、四元数
│   └── 测不变量，不是测值
├── Fixture 与参数化（33.4）
├── 雅可比测试（33.5）
│   └── 有限差分 = 独立证据链
├── 异常与错误路径（33.6）
├── 性能测试（33.7）
│   └── Google Benchmark + DoNotOptimize
├── 属性测试与 Fuzzing（33.7b）
│   ├── 属性测试：随机输入 + 数学性质
│   └── libFuzzer：覆盖率引导
├── Sanitizer（33.8）
│   ├── ASan → 内存越界
│   ├── UBSan → 未定义行为
│   └── TSan → 数据竞争
├── 调试器（33.9）
├── 静态分析（33.10）
├── 性能分析（33.11）
│   └── perf、火焰图、trace
├── CI 门禁（33.12）
└── 故障复盘（33.13）

前置自测¶

如果下面的问题答不出两题以上，建议先回顾 C++语言核心/RAII与智能指针、C++语言核心/错误处理与异常安全、并发与系统编程/线程管理与互斥同步、并发与系统编程/原子操作与内存模型、并发与系统编程/实时约束与高性能数据传递和 CMake从入门到工程化：

RAII 为什么能把异常路径和正常路径统一起来？
noexcept 对容器移动、析构和错误边界有什么影响？
数据竞争和死锁有什么区别？
为什么无锁队列也需要明确内存序和生命周期？
CMake 中测试目标为什么应该链接到被测库，而不是复制源文件？

这些问题会贯穿本章。测试不是单独的工具清单，而是对类型、资源、并发、构建和性能不变量的反复验证。

章节依赖¶

前置章节	本章使用方式
C++语言核心/类型系统与值类别推导类型系统	判断测试代码中的拷贝、引用、生命周期
C++语言核心/现代类设计与特殊成员函数特殊成员函数	测试 Rule of Zero/Five、移动后状态、异常安全
C++语言核心/RAII与智能指针 RAII	测试资源释放、作用域退出、线程 join
C++语言核心/类型转换类型转换	测试 NaN、Inf、窄化、溢出
C++语言核心/错误处理与异常安全错误处理	设计异常测试和错误返回测试
并发与系统编程/线程管理与互斥同步线程同步	写死锁和数据竞争的最小复现
并发与系统编程/原子操作与内存模型内存模型	用 TSan 检查同步假设
并发与系统编程/实时约束与高性能数据传递实时约束	区分热路径测试和离线诊断
并发与系统编程/内存分配策略与pmr-并发与系统编程/缓存优化与数据布局	测内存分配和缓存布局

回顾 C++语言核心/错误处理与异常安全：异常安全的核心不是“不抛异常”，而是“失败后对象仍满足不变量”。本章会把这个原则落到测试里：每个错误路径都要验证对象是否仍可析构、可重试、可诊断。

回顾并发与系统编程/原子操作与内存模型：并发正确性不能靠“在本机跑几次没崩”证明。本章会用 ThreadSanitizer 和最小并发测试，把内存序和同步关系变成可检查的证据。

33.1 为什么机器人项目比普通业务代码更需要质量工具 ⭐⭐¶

工程问题：机器人 bug 往往不是立刻崩溃¶

普通业务程序中的 bug 常见表现是：

场景	常见结果
参数缺失	请求失败
数据库字段错	页面报错
数组越界	程序崩溃
配置错误	服务启动失败

机器人系统中的 bug 更复杂。同一个错误可能不会立刻爆炸，而是逐渐污染状态。

例如：

错误	早期表现	后期表现
旋转矩阵不正交	位姿略有漂移	回环后地图扭曲
IMU 时间戳乱序	预积分残差变大	后端优化发散
地图点被并发删除	偶发重投影失败	数小时后段错误
浮点比较用 `==`	单元测试偶尔失败	不同 CPU 上结果分叉
配置单位混用	局部地图尺度异常	控制器振荡
点云 buffer 越界	当前帧看似正常	后续帧随机崩溃

这类错误有三个特点：

延迟暴露：错误发生的位置和崩溃的位置相隔很远。
状态污染：一次错误观测会进入地图、优化器或控制状态。
复现困难：线程调度、传感器时序和浮点顺序都会影响结果。

所以机器人 C++ 质量体系的目标不是“写完跑一遍”。目标是建立分层防线：

编译期警告
  ↓
静态分析
  ↓
单元测试
  ↓
Sanitizer 运行
  ↓
集成测试
  ↓
数据集回归
  ↓
性能基准
  ↓
线上日志与诊断

每一层都捕获不同类型的错误。把所有希望都放在“跑完整数据集”上，会太晚。

反面失败：只用真实数据集测试¶

很多 SLAM 项目一开始只有一种测试方法：

拿一段 rosbag
启动系统
看轨迹大概对不对
看 RViz 地图像不像

这种测试有价值，但不能替代单元测试。

假设轨迹偏了 10cm。可能原因包括：

外参错。
时间同步错。
重力方向错。
坐标系左右手混了。
点云滤波漏了 NaN。
ICP 残差符号反了。
鲁棒核阈值太小。
IMU bias 初始化错。
线程读到了半更新地图。
代码里某个对象移动后被继续使用。

完整数据集测试只能告诉你“系统结果坏了”。它很难告诉你“哪一层的不变量被破坏”。

所以测试要分层。

抽象不变量：每一层测试都守住一个不变量¶

质量工具真正守护的是不变量。

层级	不变量
类型与编译	接口契约能被类型系统表达
单元测试	一个函数或类满足局部数学/资源不变量
属性测试	一族输入都满足同一性质
集成测试	多模块组合后接口语义一致
数据集测试	真实传感器序列上的系统行为稳定
性能基准	热路径耗时和分配次数不退化
Sanitizer	没有越界、UAF、数据竞争、未定义行为
日志诊断	失败时有足够上下文定位原因

测试不是越多越好。测试要和不变量对应。

本质洞察：一个好测试不是”覆盖了一行代码”，而是”固定了一个系统承诺”。

这个承诺可以是数学性质、资源生命周期、线程同步关系、错误路径行为，也可以是性能上限。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为”跑通完整数据集 = 系统正确”

新手想法：”KITTI 上轨迹看起来对了，系统就没问题了。”

实际上：数据集测试是最高层测试。它能告诉你”系统结果是否可接受”，但不能告诉你”哪一层的不变量被破坏”。轨迹偏了 10 cm 可能来自外参错误、时间同步偏差、坐标系混淆、ICP 残差符号反、鲁棒核阈值不当、IMU bias 初始化错误、线程读到半更新地图等十几种原因。没有分层测试，你只能靠猜。

正确思维：数据集测试是”最终验收”，不是”唯一测试”。每个系统失败都应沉淀成一个可快速运行的局部单元测试。

⚠️ 编程陷阱：测试代码和生产代码共享同一个编译 target

错误做法：把测试文件直接加进 add_library(slam_core ... test_pose.cpp)。

现象：生产库里带着测试代码。测试框架（GoogleTest）变成了生产依赖。部署时二进制体积无故增大。

正确做法：测试代码放独立 target。add_executable(test_pose tests/test_pose.cpp) + target_link_libraries(test_pose PRIVATE slam_core GTest::gtest_main)。

练习¶

[分析题] 一次 SLAM 系统运行后轨迹漂移了 15 cm。列出至少 5 个可能的根因，并说明每个根因对应哪一层测试（单元/组件/集成/数据集）。
[设计题] 为一个 ICP 配准模块设计三层测试：(1) 单元测试验证已知变换恢复；(2) 组件测试验证与体素滤波的配合；(3) 数据集测试验证真实点云上的精度。

33.2 测试金字塔：从函数到数据集 ⭐⭐¶

工程问题：为什么不能只做端到端测试¶

端到端测试最接近真实系统。但它也最慢、最脆弱、最难定位。

可以把机器人项目测试分成五层：

                 数据集回归测试
              ┌────────────────┐
              │ rosbag / dataset│
              └────────────────┘
             集成测试
          ┌────────────┐
          │ 多模块联调 │
          └────────────┘
         组件测试
      ┌──────────────┐
      │ 前端/后端/IO │
      └──────────────┘
     单元测试
  ┌──────────────┐
  │ 函数/类/算法 │
  └──────────────┘
 静态检查
┌──────────────┐
│ 编译器/分析器 │
└──────────────┘

越底层，运行越快，定位越准。越顶层，越接近真实系统，覆盖组合行为。

类比：测试金字塔和医学检查的层次类似。静态分析像体检中的常规血液检查——快速、便宜、能筛出大量潜在问题。单元测试像专科检查——针对具体器官（模块），精确但覆盖有限。集成测试像影像学检查（CT/MRI）——能看到系统之间的关系，但成本高。数据集回归像临床试验——最接近真实，但耗时最长、最难控制变量。

层级对比¶

测试层级	运行速度	定位能力	真实度	适合发现
编译器警告	极快	高	低	类型、未初始化、隐式转换
静态分析	快	中高	低	生命周期、空指针、复杂度
单元测试	快	高	中	数学公式、局部逻辑
组件测试	中	中	中高	模块边界、资源管理
集成测试	慢	中低	高	多模块协议
数据集回归	很慢	低	最高	系统级退化

一个健康项目应该像这样安排：

提交前本地运行	CI 每次运行	夜间运行
编译	全量单元测试	大数据集回归
格式检查	Sanitizer 小数据集	长时稳定性
关键单元测试	基准烟雾测试	性能趋势分析
小型 benchmark	静态分析	多平台测试

反面失败：测试倒金字塔¶

测试倒金字塔是这样的：

大量 rosbag 测试
少量集成测试
几乎没有单元测试
没有静态分析

它的后果：

每次改动都要跑很久。
失败后很难定位。
小函数 bug 只能在大系统里暴露。
团队开始跳过测试。
测试逐渐失去约束力。

机器人系统尤其容易走向倒金字塔。因为真实数据集很有说服力，RViz 也很直观。但直观不等于可诊断。

设计原则¶

把一次系统失败向下拆：

轨迹发散
  ↓
后端残差异常
  ↓
某个因子雅可比符号错
  ↓
局部坐标扰动定义不一致
  ↓
缺少一个 6 维有限差分单元测试

拆到最后，通常是一个可以快速运行的小测试。

这就是测试设计的方向：

系统级失败要能沉淀成局部可复现测试。

33.3 GoogleTest：从断言开始建立数学不变量 ⭐⭐¶

工程问题：SLAM 中最常见的错误是“差一点”¶

几何和优化代码很少只产生两种结果。更多时候是“差一点”：

错误	表现
旋转矩阵略不正交	`R^T R` 接近但不等于 `I`
四元数没归一化	模长接近 1
雅可比符号错	有限差分差一个负号
浮点比较用等号	不同平台偶发失败
坐标系顺序错	平移误差很小但旋转方向反

所以第一条规则是：

浮点测试比较“是否足够接近”，不是比较“是否完全相等”。

最小测试结构¶

#include <gtest/gtest.h>

double add(double a, double b) {
    return a + b;
}

TEST(AddTest, AddsTwoNumbers) {
    EXPECT_DOUBLE_EQ(add(1.0, 2.0), 3.0);
}

GoogleTest 的基本单位是 TEST(SuiteName, TestName)。

宏	行为	适合
`EXPECT_*`	失败后继续执行当前测试	多个独立检查
`ASSERT_*`	失败后立刻退出当前测试	后续依赖前置条件
`EXPECT_NEAR`	绝对误差比较	浮点标量
`EXPECT_TRUE`	布尔条件	自定义 predicate
`EXPECT_THROW`	期望抛异常	错误路径
`EXPECT_NO_THROW`	期望不抛异常	正常路径

浮点比较¶

错误写法：

TEST(FloatingPointTest, BadComparison) {
    const double x = 0.1 + 0.2;
    EXPECT_EQ(x, 0.3);
}

浮点数用二进制表示。 0.1 和 0.2 都不是精确二进制小数。计算结果通常非常接近 0.3，但不一定逐位相等。

正确写法：

TEST(FloatingPointTest, UsesTolerance) {
    const double x = 0.1 + 0.2;
    EXPECT_NEAR(x, 0.3, 1e-12);
}

但容差也不能随便写。容差要来自问题尺度。

场景	容差思路
单个 double 算术	`1e-12` 到 `1e-14`
旋转矩阵正交性	取决于连续乘法次数
LiDAR 点坐标	毫米到厘米级
图像重投影	像素或亚像素
位姿轨迹	米、度或弧度

更稳的比较函数：

bool nearRelative(double a, double b, double rel, double abs) {
    const double diff = std::abs(a - b);
    if (diff <= abs) {
        // 接近 0 的数值不能只看相对误差，先用绝对误差兜底。
        return true;
    }
    // 数值尺度较大时，相对误差比固定绝对误差更稳定。
    return diff <= rel * std::max(std::abs(a), std::abs(b));
}

为什么既有相对误差又有绝对误差？

当数值很大时，相对误差更合理。
当数值接近 0 时，相对误差会失效。
两者结合才能覆盖更多尺度。

旋转矩阵测试¶

旋转矩阵的核心不变量：

\[ R^\top R = I \]

\[ \det(R) = 1 \]

第一条表示列向量正交且单位长度。第二条排除镜像反射。

代码：

#include <Eigen/Dense>
#include <gtest/gtest.h>

void expectRotationMatrix(const Eigen::Matrix3d& R, double eps) {
    // 旋转矩阵的第一层不变量：列向量应保持正交且长度为 1。
    const Eigen::Matrix3d should_be_identity = R.transpose() * R;
    EXPECT_TRUE(should_be_identity.isApprox(Eigen::Matrix3d::Identity(), eps));
    // 第二层不变量：det=1 排除镜像反射。
    EXPECT_NEAR(R.determinant(), 1.0, eps);
}

TEST(RotationMatrixTest, AngleAxisProducesValidRotation) {
    const Eigen::AngleAxisd aa(0.3, Eigen::Vector3d::UnitZ());
    const Eigen::Matrix3d R = aa.toRotationMatrix();

    expectRotationMatrix(R, 1e-12);
}

这个测试不是为了证明 Eigen 正确。它训练的是一种习惯：

测几何对象时，不只测数值结果，还要测对象所在流形的不变量。

四元数测试¶

四元数表示旋转时需要单位范数：

\[ \|q\| = 1 \]

如果四元数没有归一化，转成旋转矩阵后可能出现尺度污染。

void expectUnitQuaternion(const Eigen::Quaterniond& q, double eps) {
    EXPECT_NEAR(q.norm(), 1.0, eps);
    expectRotationMatrix(q.toRotationMatrix(), eps);
}

TEST(QuaternionTest, NormalizationIsExplicit) {
    Eigen::Quaterniond q(1.0, 0.01, 0.02, 0.03);
    q.normalize();

    expectUnitQuaternion(q, 1e-12);
}

反面失败：只测一个输入¶

错误测试：

TEST(TransformTest, AppliesTransform) {
    Eigen::Vector3d p(1.0, 0.0, 0.0);
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.translation() = Eigen::Vector3d(1.0, 2.0, 3.0);

    const Eigen::Vector3d out = T * p;

    EXPECT_TRUE(out.isApprox(Eigen::Vector3d(2.0, 2.0, 3.0)));
}

这个测试只覆盖平移。如果旋转部分完全没写，测试仍然通过。

更好的测试拆成多个不变量：

TEST(TransformTest, AppliesRotationAndTranslation) {
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.linear() =
        Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    T.translation() = Eigen::Vector3d(1.0, 2.0, 3.0);

    const Eigen::Vector3d p(1.0, 0.0, 0.0);
    const Eigen::Vector3d out = T * p;

    EXPECT_TRUE(out.isApprox(Eigen::Vector3d(1.0, 3.0, 3.0), 1e-12));
}

再加逆变换：

TEST(TransformTest, InverseCancelsTransform) {
    Eigen::Isometry3d T = makeNontrivialTransform();
    Eigen::Vector3d p(0.4, -0.2, 3.1);

    const Eigen::Vector3d recovered = T.inverse() * (T * p);

    EXPECT_TRUE(recovered.isApprox(p, 1e-12));
}

这才测试了变换的群结构。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：浮点比较使用 EXPECT_EQ

错误做法：EXPECT_EQ(0.1 + 0.2, 0.3);

现象：测试在某些平台通过，另一些平台失败。

根本原因：0.1 和 0.2 不是精确二进制小数。0.1 + 0.2 的结果是 0.30000000000000004，不等于 0.3。

正确做法：EXPECT_NEAR(0.1 + 0.2, 0.3, 1e-12); 或 EXPECT_DOUBLE_EQ()（允许 4 ULP 误差）。

💡 概念误区：认为测 Eigen 旋转矩阵是在测 Eigen 库

新手想法："Eigen 是成熟库，不需要测它。"

实际上：expectRotationMatrix(R, eps) 测试的不是 Eigen 的正确性，而是"你的代码产生的旋转矩阵是否仍满足 SO(3) 不变量"。这在 IMU 积分、连续乘法和优化迭代后尤其重要——数值误差会逐渐累积，使 \(R^T R\) 偏离 \(I\)。

练习¶

[编程题] 写一个 expectIsometry3d 辅助函数，检查 Eigen::Isometry3d 的旋转部分是否正交、行列式是否为 1。
[思考题] 为什么 EXPECT_NEAR 需要知道"问题尺度"？1 米级位移的容差和 0.001 弧度级旋转的容差应该一样吗？

33.4 Fixture：把测试初始化变成清晰对象 ⭐⭐¶

工程问题：测试准备代码重复后会漂移¶

SLAM 测试经常需要构造：

相机内参。
传感器外参。
一组 3D 点。
一组观测。
已知真值位姿。
噪声模型。

如果每个测试都手写一遍，几个月后这些准备代码会不一致。

Fixture 的作用是把共享初始化集中起来。

基本写法¶

#include <gtest/gtest.h>
#include <Eigen/Dense>

class ReprojectionFixture : public ::testing::Test {
protected:
    void SetUp() override {
        K << 500.0, 0.0, 320.0,
             0.0, 500.0, 240.0,
             0.0, 0.0, 1.0;

        T_camera_world = Eigen::Isometry3d::Identity();
        point_world = Eigen::Vector3d(0.0, 0.0, 5.0);
    }

    Eigen::Matrix3d K;
    Eigen::Isometry3d T_camera_world;
    Eigen::Vector3d point_world;
};

TEST_F(ReprojectionFixture, ProjectsPointAtCenter) {
    const Eigen::Vector2d pixel =
        project(K, T_camera_world, point_world);

    EXPECT_TRUE(pixel.isApprox(Eigen::Vector2d(320.0, 240.0), 1e-12));
}

Fixture 不应该隐藏测试意图¶

Fixture 不是把所有东西都塞进 SetUp()。一个好的 Fixture 只放“共同背景”，测试函数里仍要看得出当前测试的重点。

对比：

写法	问题
`SetUp()` 构造所有输入和期望输出	测试看不出在验证什么
测试函数里只写 `EXPECT_TRUE(result.ok)`	信息太少
Fixture 放相机内参和真值，测试里构造扰动	意图清晰

参数化测试¶

同一个不变量可以对多种算法成立。

例如点云配准接口可能有：

ICP。
GICP。
NDT。
VGICP。

可以用参数化测试：

class RegistrationParamTest
    : public ::testing::TestWithParam<std::string> {};

TEST_P(RegistrationParamTest, RecoversKnownTransform) {
    const std::string method = GetParam();
    auto registration = createRegistration(method);

    const auto source = makeSourceCloud();
    const auto target = transformCloud(source, knownTransform());

    const auto result = registration->align(source, target);

    EXPECT_TRUE(result.converged);
    EXPECT_TRUE(result.T.isApprox(knownTransform(), 1e-3));
}

INSTANTIATE_TEST_SUITE_P(
    AllMethods,
    RegistrationParamTest,
    ::testing::Values("icp", "gicp", "ndt"));

这类测试能固定接口契约：

不管后端实现怎么变，同一个已知刚体变换都应该被恢复。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Fixture 的 SetUp() 抛异常导致测试跳过而非失败

错误做法：SetUp() 中加载一个测试文件，文件不存在时抛异常。

现象：GoogleTest 报 SetUp() 失败，但不运行测试体，也不输出具体原因。看起来像"测试被跳过"，实际上是环境问题。

正确做法：在 SetUp() 中使用 ASSERT_TRUE(std::filesystem::exists(test_data_path)) << "Missing test data: " << test_data_path; 显式报告原因。

💡 概念误区：认为 Fixture 越大越好，把所有测试都放进一个 Fixture

新手想法："一个大 Fixture 可以避免重复代码。"

实际上：过大的 Fixture 让每个测试都不必要地构造大量对象，拖慢运行速度，更重要的是让测试意图变得模糊——读者看不出每个测试到底依赖 Fixture 的哪些成员。

正确做法：按不变量分组。相机投影测试用一个 Fixture，ICP 测试用另一个。共享的辅助函数放到独立的 helper 文件中。

练习¶

[编程题] 设计一个 PoseGraphFixture，包含一个小型位姿图（5 个顶点、6 条边）。写两个测试：(1) 优化后所有边残差低于阈值；(2) 移除一条边后重新优化，其他边残差不应剧烈变化。
[思考题] 参数化测试中，如果某个算法在特定参数组合下一直失败，你会把它标记为 DISABLED_ 还是修复它？讨论两种选择的风险。

33.5 雅可比测试：用有限差分保护优化代码 ⭐⭐⭐¶

工程问题：雅可比错一列，优化器可能仍然跑¶

非线性最小二乘通常写成：

\[ \min_x \frac{1}{2}\|r(x)\|^2 \]

Gauss-Newton 需要雅可比：

\[ J = \frac{\partial r}{\partial x} \]

如果 \(J\) 错了，优化器不一定立刻崩溃。它可能：

收敛变慢。
在某些初值下发散。
对某些数据集正确，对另一些数据集错误。
用更小步长勉强收敛，掩盖问题。

所以雅可比必须单独测试。

有限差分思想¶

一维函数：

\[ f'(x) \approx \frac{f(x+\epsilon)-f(x-\epsilon)}{2\epsilon} \]

多维残差：

\[ J_{:,i} \approx \frac{r(x+\epsilon e_i)-r(x-\epsilon e_i)}{2\epsilon} \]

为什么用中心差分？

方法	公式	截断误差
前向差分	\((f(x+\epsilon)-f(x))/\epsilon\)	\(O(\epsilon)\)
中心差分	\((f(x+\epsilon)-f(x-\epsilon))/(2\epsilon)\)	\(O(\epsilon^2)\)

中心差分多算一次函数，但更准确。

测试框架¶

Eigen::MatrixXd numericJacobian(
    const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& residual,
    const Eigen::VectorXd& x,
    double eps) {
    const Eigen::VectorXd r0 = residual(x);
    Eigen::MatrixXd J(r0.size(), x.size());

    for (int i = 0; i < x.size(); ++i) {
        Eigen::VectorXd xp = x;
        Eigen::VectorXd xm = x;
        // 只扰动第 i 个自由度，用中心差分降低截断误差。
        xp[i] += eps;
        xm[i] -= eps;

        J.col(i) = (residual(xp) - residual(xm)) / (2.0 * eps);
    }

    return J;
}

解析雅可比测试：

TEST(JacobianTest, ReprojectionJacobianMatchesFiniteDifference) {
    ReprojectionFactor factor(makeCamera(), makeObservation());

    Eigen::VectorXd x = makeNontrivialState();
    Eigen::VectorXd r;
    Eigen::MatrixXd J;

    factor.evaluate(x, &r, &J);

    // 数值雅可比独立于解析推导，是保护优化代码的第二条证据链。
    const Eigen::MatrixXd J_num =
        numericJacobian([&](const Eigen::VectorXd& state) {
            Eigen::VectorXd residual;
            factor.evaluate(state, &residual, nullptr);
            return residual;
        }, x, 1e-6);

    EXPECT_TRUE(J.isApprox(J_num, 1e-5));
}

易错点：扰动空间不是总在欧氏空间¶

位姿属于 \(SE(3)\)，不能简单把 4x4 矩阵每个元素加 \(\epsilon\)。正确做法是在李代数上扰动：

\[ T(\delta) = \exp(\delta^\wedge)T \]

或：

\[ T(\delta) = T\exp(\delta^\wedge) \]

左扰动和右扰动得到的雅可比不同。测试必须和实现约定一致。

示意代码：

Eigen::Matrix<double, 6, 1> delta =
    Eigen::Matrix<double, 6, 1>::Zero();
delta[i] = eps;

const Sophus::SE3d T_plus =
    Sophus::SE3d::exp(delta) * T;

const Sophus::SE3d T_minus =
    Sophus::SE3d::exp(-delta) * T;

如果实现使用右扰动，测试也要改成右乘。

本质洞察：雅可比测试不仅验证公式，还验证”扰动定义”。

很多优化 bug 不是微分算错，而是解析雅可比和优化器使用的局部参数化不一致。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：SE(3) 位姿的有限差分在欧氏空间上扰动

错误做法：把 4x4 变换矩阵的每个元素加 epsilon。

现象：有限差分结果和解析雅可比完全不匹配。

根本原因：SE(3) 不是欧氏空间。在矩阵元素上加 epsilon 后，结果不再是有效的刚体变换（\(R^T R \neq I\)）。

正确做法：在李代数 \(\mathfrak{se}(3)\) 的 6 维空间上扰动：\(T(\delta) = \exp(\delta^\wedge) T\)（左扰动）或 \(T(\delta) = T \exp(\delta^\wedge)\)（右扰动）。

🧠 思维陷阱：认为”优化器能收敛就说明雅可比对了”

新手想法：”优化结果合理，雅可比应该没问题。”

实际上：错误的雅可比也可能让优化器收敛——只是收敛更慢、需要更多迭代、或者只在某些初值下正确。真正危险的是”在大多数数据集上收敛，在某个边界条件下发散”。有限差分测试是独立于优化器的第二条证据链。

练习¶

[编程题] 为一个 SE(3) 左扰动的重投影因子写有限差分雅可比测试。使用 Sophus 的 SE3d::exp() 做扰动。
[思考题] 左扰动和右扰动得到的雅可比不同。如果解析实现用左扰动，但有限差分用右扰动，测试会通过吗？为什么？
[跨章综合题] 回顾设计模式与高级惯用法中 Ceres 优化器使用 AutoDiffCostFunction 自动微分。如果手写解析雅可比和自动微分结果不一致，你会信任哪个？设计一个三方验证方案（解析 vs. 自动微分 vs. 有限差分）。

33.6 异常与错误路径测试 ⭐⭐¶

过渡¶

前几节覆盖了正常逻辑路径的测试——数学不变量、浮点比较、参数化、雅可比。但系统的可靠性往往不取决于正常路径写得多好，而取决于异常路径处理得多完善。回顾 C++语言核心/错误处理与异常安全：异常安全的核心不是"不抛异常"，而是"失败后对象仍满足不变量"。本节把这个原则变成可执行的测试。

工程问题：错误路径最少运行，却最容易出事¶

正常路径每天都会跑。错误路径可能几个月才跑一次。

典型错误路径：

配置文件缺字段。
相机内参非法。
点云为空。
时间戳乱序。
文件读取失败。
CUDA 设备不可用。
外参矩阵不可逆。
优化器不收敛。

这些路径如果不测，往往会在现场运行时暴露。

异常测试¶

TEST(ConfigTest, RejectsNegativeVoxelSize) {
    Config config;
    config.voxel_size = -0.1;

    EXPECT_THROW(validate(config), std::invalid_argument);
}

如果还要检查错误消息：

TEST(ConfigTest, ErrorMessageContainsParameterName) {
    Config config;
    config.voxel_size = -0.1;

    try {
        validate(config);
        FAIL() << "validate should throw";
    } catch (const std::invalid_argument& e) {
        EXPECT_THAT(std::string(e.what()),
                    ::testing::HasSubstr("voxel_size"));
    }
}

错误消息不是装饰。它是调试接口。

资源不变量测试¶

错误路径还要检查资源是否释放。例如订阅句柄：

TEST(SubscriptionTest, UnsubscribesOnException) {
    FakeEventBus bus;

    {
        Subscription sub = bus.subscribe([](const Frame&) {});
        EXPECT_EQ(bus.subscriptionCount(), 1);
    }

    EXPECT_EQ(bus.subscriptionCount(), 0);
}

再测移动：

TEST(SubscriptionTest, MoveTransfersOwnership) {
    FakeEventBus bus;

    Subscription a = bus.subscribe([](const Frame&) {});
    Subscription b = std::move(a);

    EXPECT_EQ(bus.subscriptionCount(), 1);
}

这类测试对应 C++语言核心/RAII与智能指针的 RAII 不变量：

资源的释放必须绑定到对象生命周期，不能依赖调用者记得手动调用。

死亡测试¶

有些错误属于不可恢复的契约违反。例如内部断言：

TEST(InternalInvariantTest, DeathOnNonFinitePose) {
    EXPECT_DEATH({
        PoseGraph graph;
        graph.addPose(makePoseWithNaN());
    }, "finite");
}

死亡测试适合内部不变量。公共 API 更推荐返回错误或抛异常，而不是直接终止。

33.7 Google Benchmark：性能测试不是打印时间 ⭐⭐¶

工程问题：`std::chrono` 手写计时很容易测错¶

常见错误：

auto start = std::chrono::steady_clock::now();
voxelFilter(points);
auto end = std::chrono::steady_clock::now();
std::cout << elapsed(start, end) << "\n";

这段代码可能有问题：

只跑一次，受冷启动影响。
输入太小，噪声大于信号。
编译器把结果优化掉。
没有区分预处理、分配、核心计算。
没有统计分布。
没有固定 CPU 频率和数据规模。

Benchmark 的目标不是“得到一个数字”。目标是得到可重复、可比较、可解释的数字。

最小 benchmark¶

#include <benchmark/benchmark.h>

static void BM_VoxelFilter(benchmark::State& state) {
    const int n = static_cast<int>(state.range(0));
    // 数据准备放在循环外，避免把随机数生成和分配成本算进核心算法。
    std::vector<PointXYZI> points = makeRandomCloud(n);

    for (auto _ : state) {
        auto filtered = voxelFilter(points, 0.2);
        // 防止编译器发现结果未使用后删除整个计算。
        benchmark::DoNotOptimize(filtered);
    }
}

BENCHMARK(BM_VoxelFilter)->Range(1000, 1000000);

BENCHMARK_MAIN();

benchmark::DoNotOptimize 告诉编译器：

这个结果对外部可见，不要把计算删掉。

否则如果 filtered 没有被使用，优化器可能直接删除整个调用。

反面失败：把准备数据算进核心算法¶

如果每次循环都生成随机点云：

for (auto _ : state) {
    auto points = makeRandomCloud(n);
    auto filtered = voxelFilter(points, 0.2);
    benchmark::DoNotOptimize(filtered);
}

测到的是：

随机数生成 + vector 分配 + 点云填充 + voxel filter

不是单独的 voxel filter。

如果你要测端到端，那这没问题。但名字应写成 BM_VoxelFilterEndToEnd。如果要测核心算法，就应把数据准备放在循环外。

分配次数也是性能指标¶

SLAM 热路径中，分配次数常常比平均耗时更重要。因为分配会带来长尾延迟。

可以设计两组 benchmark：

版本	目的
每帧创建容器	作为朴素基线
复用 workspace	验证并发与系统编程/内存分配策略与pmr 的分配优化

示意：

static void BM_VoxelFilterWithWorkspace(benchmark::State& state) {
    const int n = static_cast<int>(state.range(0));
    const std::vector<PointXYZI> points = makeRandomCloud(n);
    VoxelWorkspace workspace;
    // workspace 在循环外预留容量，测试的是复用后的热路径成本。
    workspace.reserve(points.size());

    for (auto _ : state) {
        auto filtered = voxelFilter(points, 0.2, &workspace);
        benchmark::DoNotOptimize(filtered);
    }
}

解释 benchmark 结果¶

假设输出：

输入点数	原始版本	workspace 版本
10k	0.8ms	0.7ms
100k	9.5ms	6.2ms
1M	140ms	82ms

不能只说“workspace 更快”。要解释原因：

点数越大，临时容器越多。
哈希桶、输出点、索引数组反复分配。
workspace 把容量保留在对象生命周期内。
大输入下分配成本和缓存局部性影响更明显。

Benchmark 不是结论。 Benchmark 是证据。

33.7b 属性测试与 Fuzzing：超越手写用例 ⭐⭐⭐¶

动机：手写测试只覆盖你想到的输入¶

前面的 GoogleTest 用例都是开发者手动选择的输入。这意味着测试只能覆盖你已经预见到的情况。但机器人系统面对的输入空间远比你能想到的更大——传感器噪声、退化场景、极端数值、恶意数据包。如果一个 ICP 实现在所有手写测试上通过，但在某个特殊的点云分布上崩溃，手写测试永远发现不了。

属性测试（Property-based Testing）和模糊测试（Fuzzing）从不同角度解决这个问题。属性测试在随机输入上验证数学性质是否成立，Fuzzing 在随机输入上寻找导致崩溃或异常的输入。二者的共同思想是：让机器帮你探索输入空间。

类比：手写测试像是用手电筒照几个角落，属性测试像是开灯扫一整个房间，Fuzzing 像是在黑暗中摸索寻找每一条裂缝。

如果不做属性测试会怎样¶

考虑一个 SE(3) 逆运算测试。手写版本可能这样：

TEST(SE3Test, InverseRecoversIdentity) {
    Sophus::SE3d T(Eigen::Quaterniond(1, 0.1, 0.2, 0.3).normalized(),
                   Eigen::Vector3d(1, 2, 3));
    auto result = T * T.inverse();
    EXPECT_TRUE(result.matrix().isApprox(Eigen::Matrix4d::Identity(), 1e-10));
}

这只测了一个位姿。如果逆运算在纯旋转（平移为零）时有 bug，这个测试发现不了。如果在接近 \(\pi\) 旋转时数值不稳定，这个测试也发现不了。属性测试用随机位姿反复验证同一个性质：

#include <random>
#include <gtest/gtest.h>
#include <sophus/se3.hpp>

Sophus::SE3d randomSE3(std::mt19937& rng) {
    std::uniform_real_distribution<double> dist(-10.0, 10.0);
    std::uniform_real_distribution<double> angle(-M_PI, M_PI);
    Eigen::Vector3d axis = Eigen::Vector3d(dist(rng), dist(rng), dist(rng)).normalized();
    Eigen::AngleAxisd aa(angle(rng), axis);
    Eigen::Vector3d t(dist(rng), dist(rng), dist(rng));
    return Sophus::SE3d(aa.toRotationMatrix(), t);
}

TEST(SE3PropertyTest, InverseIsGroupInverse) {
    std::mt19937 rng(42);  // 固定种子，保证可复现
    for (int i = 0; i < 1000; ++i) {
        Sophus::SE3d T = randomSE3(rng);
        // 群逆元性质：T * T^{-1} = I
        auto result = T * T.inverse();
        EXPECT_TRUE(result.matrix().isApprox(
            Eigen::Matrix4d::Identity(), 1e-10))
            << "Failed at iteration " << i;
    }
}

1000 次随机位姿比 1 个手选位姿更有可能触发边界情况。如果某次迭代失败，固定种子保证可以精确复现。

libFuzzer：让编译器帮你找崩溃¶

libFuzzer 是 LLVM 内置的覆盖率引导模糊测试引擎。它不断生成随机输入，执行目标函数，用代码覆盖率反馈指导后续输入的变异——越是能走到新分支的输入越会被保留和进一步变异。

对机器人项目来说，最适合 Fuzzing 的目标是解析器。配置文件解析、g2o 文件解析、PCD 文件头解析、自定义二进制协议解析——这些代码接收外部输入，如果没有充分的边界检查，恶意或损坏的输入可能导致越界、无限循环或未定义行为。

一个最小的 Fuzz 目标：

// fuzz_tum_parser.cpp
#include <cstdint>
#include <cstddef>
#include <string>

// 被测函数：解析一行 TUM 轨迹
bool parseTUMLine(const std::string& line, double& ts,
                  double& tx, double& ty, double& tz,
                  double& qx, double& qy, double& qz, double& qw);

extern "C" int LLVMFuzzerTestOneInput(const uint8_t* data, size_t size) {
    // 将随机字节解释为字符串，送给解析器
    std::string line(reinterpret_cast<const char*>(data), size);
    double ts, tx, ty, tz, qx, qy, qz, qw;
    // 解析器应当安全处理任何输入，不崩溃、不越界
    parseTUMLine(line, ts, tx, ty, tz, qx, qy, qz, qw);
    return 0;
}

编译和运行：

# 使用 clang 编译，启用 Fuzzing 和 ASan
clang++ -g -fsanitize=fuzzer,address -o fuzz_tum fuzz_tum_parser.cpp parser.cpp
# 运行 Fuzzer，corpus 目录存放有价值的输入
mkdir -p corpus
./fuzz_tum corpus/ -max_total_time=300

libFuzzer 会在 5 分钟内尝试数百万种输入变异。如果解析器在某个输入上崩溃，Fuzzer 会保存导致崩溃的输入到文件，供调试使用。

反事实推理：如果不做 Fuzzing 会怎样？解析器可能在实验室的标准数据集上永远不出问题，但在客户的损坏日志文件或网络传输截断的数据上崩溃。Fuzzing 在几分钟内就能发现这类隐藏缺陷，而手动构造恶意输入需要开发者对每一个解析分支都有深入理解。

CI/CD 集成：把质量要求变成自动化流水线¶

CI（持续集成）不只是"自动跑一下测试"。一个成熟的机器人项目 CI 应该包含多个阶段，每个阶段用不同工具捕获不同类型的错误：

# .github/workflows/ci.yml 示例结构
name: Quality Gates
on: [push, pull_request]

jobs:
  format-check:
    runs-on: ubuntu-24.04
    steps:
      - uses: actions/checkout@v4
      - name: Check formatting
        run: |
          find src include -name "*.cpp" -o -name "*.hpp" | \
            xargs clang-format --dry-run --Werror

  build-and-test:
    runs-on: ubuntu-24.04
    strategy:
      matrix:
        sanitizer: [none, asan, tsan]
    steps:
      - uses: actions/checkout@v4
      - name: Configure
        run: |
          cmake -S . -B build \
            -DCMAKE_BUILD_TYPE=RelWithDebInfo \
            -DENABLE_ASAN=${{ matrix.sanitizer == 'asan' }} \
            -DENABLE_TSAN=${{ matrix.sanitizer == 'tsan' }}
      - name: Build
        run: cmake --build build -j$(nproc)
      - name: Test
        run: ctest --test-dir build --output-on-failure

  static-analysis:
    runs-on: ubuntu-24.04
    steps:
      - uses: actions/checkout@v4
      - name: clang-tidy
        run: |
          cmake -S . -B build -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
          run-clang-tidy -p build src/

  benchmark-smoke:
    runs-on: ubuntu-24.04
    steps:
      - uses: actions/checkout@v4
      - name: Run benchmarks
        run: |
          cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
          cmake --build build --target bm_voxel_filter
          ./build/bm_voxel_filter --benchmark_min_time=1s

CI 的关键设计原则：

原则	做法	原因
快反馈	格式检查和编译放最前面	格式错误 30 秒就能发现
分层运行	单元测试每次 push，大数据集夜间	避免 CI 时间过长导致团队跳过
Sanitizer 矩阵	ASan 和 TSan 分开跑	两者不能同时启用
性能烟雾测试	Benchmark 不检查绝对数值	只检查是否能运行、是否超时
可复现	固定编译器版本、固定依赖版本	避免"在我机器上能过"

本质洞察：CI 不是在做"自动化测试"——它是在用代码表达质量标准。每一条 CI 规则都是团队对"什么是可接受的提交"的共识。没有 CI 的团队靠个人纪律；有 CI 的团队靠系统保障。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"随机测试不可靠，因为每次运行结果不同"

新手想法："属性测试用随机输入，每次运行可能测不同的东西，不可靠。"

实际上：属性测试通过固定随机种子保证可复现性。种子 42 产生的 1000 个位姿每次都完全相同。如果测试失败，用同一个种子就能精确复现。随机性只在第一次发现 bug 时起作用——一旦发现，就变成了一个确定性的回归测试。

⚠️ 编程陷阱：Fuzz 目标函数抛异常但未捕获

错误做法：Fuzz 目标函数内部调用了可能抛异常的解析代码，但没有 try/catch。

现象：libFuzzer 把每个 std::exception 都当作崩溃报告，产生大量误报。

正确做法：在 LLVMFuzzerTestOneInput 内部用 try/catch 包裹被测函数。解析器抛异常是正常行为（拒绝非法输入），只有段错误、越界等非异常崩溃才是真正的 bug。

练习¶

[编程题] 为 SE(3) 组合运算写一个属性测试：验证结合律 \((A \cdot B) \cdot C = A \cdot (B \cdot C)\) 在 500 个随机位姿三元组上成立。
[编程题] 为一个简单的 INI 配置文件解析器编写 libFuzzer 目标。验证解析器在任意字节输入下不崩溃。
[设计题] 为你的项目设计一个 GitHub Actions CI 流水线，包含格式检查、编译、单元测试（三种 Sanitizer）、静态分析和 Benchmark 烟雾测试。说明每个阶段捕获什么类型的错误。

33.8 Sanitizer：让未定义行为尽早爆炸 ⭐⭐¶

工程问题：C++ 错误不一定立刻报错¶

C++ 的危险在于很多错误属于未定义行为。程序可以：

立刻崩溃。
看起来正常。
只在 Release 崩溃。
只在某个 CPU 崩溃。
改一行日志后不崩。

Sanitizer 的作用是把隐蔽错误变成明确错误。

类比：Sanitizer 像是给程序装了一套"症状放大器"。就像医学上的造影剂让隐藏的病灶在 X 光下变得清晰可见，Sanitizer 让隐蔽的内存错误和竞争条件在运行时立刻暴露为明确的错误报告，而不是等到数月后在生产环境中以不可预测的方式崩溃。

反事实推理：如果不用 Sanitizer 会怎样？一个堆越界写入可能在本地 Debug 模式下表现为"偶发崩溃"，Release 模式下表现为"数据轻微错误"，在另一台机器上表现为"完全正常"。没有 Sanitizer，你可能花一周时间在错误的方向上排查，最终靠运气发现问题。ASan 可以在第一次越界写入时就告诉你精确的地址、调用栈和分配信息。

ASan：内存越界和释放后使用¶

启用：

target_compile_options(slam_core PRIVATE -fsanitize=address -fno-omit-frame-pointer)
target_link_options(slam_core PRIVATE -fsanitize=address)

典型错误：

std::vector<PointXYZI> points(10);
points[10].x = 1.0f;

ASan 会报告 heap-buffer-overflow，并给出调用栈。

释放后使用：

PointXYZI* raw = nullptr;
{
    std::vector<PointXYZI> points(10);
    raw = points.data();
}
raw[0].x = 1.0f;

这类错误在机器人代码中经常来自：

保存了 vector::data() 指针。
回调中捕获了局部对象引用。
Eigen::Map 指向已经释放的 buffer。
CUDA 异步拷贝使用了已销毁 host buffer。

UBSan：未定义行为¶

启用：

target_compile_options(slam_core PRIVATE -fsanitize=undefined)
target_link_options(slam_core PRIVATE -fsanitize=undefined)

常见发现：

错误	例子
signed overflow	`int index = x * y` 溢出
invalid shift	左移超过位宽
null reference	解引用空指针后绑定引用
vptr 错误	多态对象生命周期已结束仍调用虚函数

SLAM 中尤其常见的是索引溢出。点云、图像、体素 key 经常把多个坐标编码成一个整数。如果没有边界检查，负坐标、大地图和极端输入会触发未定义行为。

TSan：数据竞争¶

启用：

target_compile_options(slam_core PRIVATE -fsanitize=thread)
target_link_options(slam_core PRIVATE -fsanitize=thread)

数据竞争定义：

两个线程访问同一内存位置，至少一个是写，且没有同步关系。

典型错误：

struct SharedState {
    Eigen::Isometry3d pose;
};

SharedState state;

std::thread writer([&] {
    state.pose.translation().x() += 1.0;
});

std::thread reader([&] {
    std::cout << state.pose.translation().x() << "\n";
});

// 生命周期边界：
// 这里故意保留数据竞争给 TSan 捕获，但线程对象本身仍必须 join。
// 否则测试会先因为 std::thread 析构时仍 joinable 而 std::terminate。
writer.join();
reader.join();

即使 double 读写在某个平台看起来是原子的，这仍然是数据竞争。 TSan 会指出两个访问位置。

Sanitizer 组合边界¶

工具	能否常用	注意
ASan	开发期强烈推荐	内存开销较高
UBSan	开发期推荐	可与 ASan 组合
TSan	专门并发测试	通常不能与 ASan 同时用
MSan	更难部署	所有依赖也要插桩

机器人项目可以设置多个构建配置：

Debug
RelWithDebInfo
AsanDebug
UbsanDebug
TsanDebug
Release

不要在实时性能测试中使用 Sanitizer 数字。 Sanitizer 改变运行时开销。它用于找错，不用于评估最终性能。

33.9 GDB/LLDB：从崩溃点回到原因 ⭐⭐¶

工程问题：崩溃位置不等于错误位置¶

一个 SLAM 程序可能在 std::vector 析构时崩溃。这并不说明 vector 有问题。更可能是前面某处越界写坏了堆元数据。

调试器的第一步不是“看崩溃那一行”。而是建立时间链：

程序在哪里崩溃
  ↓
谁调用到这里
  ↓
对象是谁
  ↓
对象什么时候变坏
  ↓
哪个写操作破坏了不变量

基本命令¶

命令	用途
`run`	启动程序
`bt`	查看调用栈
`frame 3`	切换栈帧
`p variable`	打印变量
`info threads`	查看线程
`thread 2`	切换线程
`break file.cpp:123`	设置断点
`watch variable`	监视变量写入
`catch throw`	捕获异常抛出
`continue`	继续运行

多线程调试¶

SLAM 常有：

Tracking 线程。
Mapping 线程。
LoopClosing 线程。
Visualization 线程。
ROS executor 线程。

崩溃时先看：

info threads
thread apply all bt

thread apply all bt 能看到所有线程的调用栈。如果程序卡住，而不是崩溃，最常见是死锁。

死锁栈通常长这样：

Thread 1: waiting on map_mutex
Thread 2: waiting on queue_mutex
Thread 3: waiting on map_mutex

然后检查锁顺序。如果一个路径先拿 map_mutex 再拿 queue_mutex，另一个路径反过来，就可能死锁。

Watchpoint：找是谁改坏了变量¶

如果某个变量突然变成 NaN：

watch pose.translation_.x
continue

watchpoint 会在变量被写时停下。

它适合：

追踪状态被谁修改。
找到数组越界写。
找到意外的 move 后清空。
找到并发写入。

但 watchpoint 数量有限，开销也高。它是定位工具，不是常规测试。

调试器与 Sanitizer 的关系¶

Sanitizer 告诉你“发生了什么错误”。调试器帮助你理解“为什么走到这里”。

常见组合：

用 ASan 找到越界位置。
在该位置附近加断点。
用 GDB 看输入数据和对象状态。
把发现转成单元测试。

工具链最终要回到测试。否则同一个 bug 以后还会回来。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：在 Release 模式下调试，断点和变量被优化掉

错误做法：使用 -O2 编译的二进制进行调试。

现象：设置断点后程序跳过了断点、变量显示为 <optimized out>、调用栈看起来不连续。

根本原因：编译器优化（内联、循环展开、寄存器分配）改变了代码的执行顺序和变量存储位置。调试器依赖 DWARF 调试信息将机器指令映射回源码，但优化后这种映射不再一一对应。

正确做法：调试时使用 -O0 -g 或 -Og -g（-Og 在保留调试体验的同时启用不影响调试的优化）。对性能相关的 bug，使用 RelWithDebInfo（-O2 -g）编译，但要接受部分变量不可观察。

💡 概念误区：认为 watchpoint 可以替代 Sanitizer

新手想法："用 watchpoint 监视内存就能找到所有越界写入。"

实际上：硬件 watchpoint 数量有限（x86 上通常只有 4 个），且只能监视特定地址。ASan 能检测整个地址空间的越界访问，覆盖范围远超 watchpoint。调试器和 Sanitizer 是互补工具：Sanitizer 发现问题位置，调试器理解问题原因。

练习¶

[实践题] 在一个 SLAM 程序中制造一个死锁（两个线程以不同顺序获取两个 mutex），用 thread apply all bt 找到死锁的两个线程并画出锁等待图。
[思考题] 为什么 catch throw 比在每个 catch 语句设置断点更有效？考虑一个有 50 个 catch 语句的大型项目。

33.10 静态分析：把风格问题和潜在错误前移 ⭐⭐¶

工程问题：代码还没运行，很多错误已经能看出来¶

静态分析不运行程序。它检查源代码结构。

可以发现：

未初始化变量。
悬空引用风险。
不必要拷贝。
错误的移动。
容器越界模式。
虚析构缺失。
复杂度过高函数。
现代 C++ 替代写法。

编译器警告¶

建议基础警告：

target_compile_options(slam_core PRIVATE
    -Wall
    -Wextra
    -Wpedantic
    -Wconversion
    -Wshadow
)

-Werror 是否默认开启要谨慎。

场景	建议
项目内部代码	CI 可开启 `-Werror`
第三方头文件	不应受你的 `-Werror` 影响
跨平台编译	分平台控制警告
教学代码	先解释警告原因，再决定是否升级为错误

clang-tidy¶

典型配置：

Checks: >
  bugprone-*,
  performance-*,
  modernize-*,
  readability-*,
  cppcoreguidelines-*,
  -readability-magic-numbers
WarningsAsErrors: ''
HeaderFilterRegex: 'src/|include/'

常见检查：

检查	价值
`bugprone-use-after-move`	移动后继续使用
`bugprone-exception-escape`	析构函数或 noexcept 函数抛异常
`performance-unnecessary-value-param`	参数不必要按值传递
`modernize-use-nullptr`	旧式空指针
`cppcoreguidelines-owning-memory`	裸 owning 指针

反面失败：工具结果不分层¶

如果一次启用所有检查，项目可能出现几千条提示。团队会很快忽略它们。

更好的方式是分层：

先启用 bugprone 和性能相关检查。
修掉明确 bug。
再启用现代化建议。
对有争议的风格项逐项讨论。
最后只让稳定规则进入 CI。

质量工具要服务工程目标。不是把代码改成工具喜欢的样子。

⚠️ 常见陷阱¶

🧠 思维陷阱：一次性启用所有 clang-tidy 检查，然后被海量警告淹没

新手想法："把所有检查都打开，一次修完。"

实际上：一个中型 SLAM 项目开启全部 clang-tidy 检查可能产生几千条警告，其中很多是风格偏好（如 modernize-use-trailing-return-type）而非真正的 bug。团队面对这么多警告会选择无视，工具失去约束力。

正确做法：从 bugprone-* 和 performance-* 开始，修掉所有真正的 bug 后再逐步启用其他检查组。每次 PR 只引入一组新检查。

练习¶

[实践题] 在一个 SLAM 项目上运行 clang-tidy 的 bugprone-* 检查，列出最高频的 3 类警告，并解释每类警告的潜在风险。
[设计题] 设计一个 .clang-tidy 配置，对 src/ 下的项目代码严格检查，但对 third_party/ 下的第三方代码跳过检查。

33.11 性能分析：先找热点，再谈优化 ⭐⭐¶

工程问题：性能直觉经常错¶

开发者常猜：

最近邻搜索最慢。
Eigen 矩阵乘法最慢。
日志输出最慢。
ROS 消息转换最慢。

猜测可能对，也可能错。优化前必须 profile。

perf¶

Linux 下常用：

perf record -g ./slam_node
perf report

-g 记录调用栈。这样不仅知道哪个函数慢，还知道它从哪里被调用。

常见热点类型：

热点	可能原因
`std::unordered_map::find`	哈希质量差或 rehash
`malloc` / `free`	热路径频繁分配
`Eigen::internal::*`	矩阵计算或临时表达式
`pcl::KdTreeFLANN::nearestKSearch`	最近邻搜索
`memcpy`	大对象复制
logging sink	日志过多或同步写文件

火焰图¶

火焰图的横向宽度表示耗时比例。它适合回答：

总时间花在哪些调用链。
一个热点来自哪条路径。
优化是否改变了主要瓶颈。

如果火焰图里出现大量 std::shared_ptr 引用计数操作，说明对象所有权可能在热路径中过度共享。如果出现大量分配器函数，说明需要并发与系统编程/内存分配策略与pmr 的 workspace 或对象池。

trace：看时间线¶

SLAM 是 pipeline。单纯函数耗时不够。还要看时间线：

frame 1024:
  preprocess  2.1ms
  tracking    8.4ms
  mapping     23.0ms
  publish     1.2ms

Chrome trace 事件格式：

{
  "name": "tracking",
  "ph": "X",
  "ts": 100000,
  "dur": 8400,
  "pid": 1,
  "tid": 2
}

用 trace 可以发现：

哪个阶段超时。
队列是否积压。
多线程是否真的并行。
GPU 任务是否阻塞 CPU。
可视化是否影响定位。

⚠️ 常见陷阱¶

🧠 思维陷阱：凭直觉优化而不先 profile

新手想法："最近邻搜索肯定是瓶颈，我来优化 KD-Tree。"

实际上：profile 结果经常出人意料。真正的热点可能是 std::unordered_map 的 rehash、临时 vector 的反复分配、或者日志格式化。在不 profile 的情况下优化，可能花了一周优化一个只占 5% 时间的函数，而忽略了占 40% 时间的分配热点。

正确做法：先用 perf record -g 或火焰图定位热点，确认哪些函数占总时间的大头，再优化。优化后再 profile 确认效果。

⚠️ 编程陷阱：在 Sanitizer 模式下测性能

错误做法：在 ASan 编译的二进制上运行 Benchmark，得出"voxel filter 需要 50ms"。

现象：数字比实际慢 2-5 倍。

根本原因：ASan 在每次内存访问前后插入了检查代码，运行时开销 2-3 倍。TSan 的开销更大，可达 5-15 倍。

正确做法：性能测试用 -O2 Release 编译，不带任何 Sanitizer。Sanitizer 用于找 bug，不用于评估性能。

练习¶

[实践题] 用 perf record -g 和火焰图分析一个 SLAM 程序的性能热点。找到耗时最多的 3 个函数调用链。
[分析题] 如果火焰图中 malloc/free 占了 30% 的时间，你会如何优化？列出至少 3 种策略（提示：预分配、对象池、pmr）。

33.12 CI 门禁：把质量要求固化为流程 ⭐⭐¶

工程问题：只靠个人习惯不能长期稳定¶

一个人可以记得提交前运行测试。多人项目不能靠记忆。

CI 的目标是把质量规则固化：

push
  ↓
configure
  ↓
build
  ↓
unit tests
  ↓
sanitizer tests
  ↓
static analysis
  ↓
benchmark smoke

基础门禁¶

门禁	目的
格式检查	减少风格争论
编译警告	阻止明显风险
单元测试	固定局部不变量
ASan/UBSan	抓内存和未定义行为
TSan 小测试	抓并发数据竞争
clang-tidy	抓静态风险
小数据集回归	抓系统组合退化

CMake + CTest¶

enable_testing()

add_executable(test_geometry
    tests/test_rotation.cpp
    tests/test_jacobian.cpp
)

target_link_libraries(test_geometry
    PRIVATE
        slam_core
        GTest::gtest_main
)

add_test(NAME test_geometry COMMAND test_geometry)

运行：

ctest --output-on-failure

--output-on-failure 很重要。 CI 失败时要看到失败细节。

测试命名¶

测试名应该表达不变量：

不推荐	推荐
`Test1`	`RejectsNegativeVoxelSize`
`IcpWorks`	`RecoversKnownRigidTransform`
`PoseTest`	`InverseCancelsComposition`
`QueueTest`	`PreservesOrderForSingleProducerSingleConsumer`

好名字能减少阅读成本。

33.13 故障复盘：把一次事故变成长期资产 ⭐⭐¶

工程问题：同一个 bug 不应该出现第二次¶

机器人项目中，一次 bug 复盘应该产出至少一个测试或工具规则。

复盘流程：

现象
  ↓
最小复现
  ↓
根因定位
  ↓
不变量提炼
  ↓
测试固化
  ↓
诊断信息增强

示例：地图偶发崩溃¶

现象：

系统运行 2 小时后偶发崩溃
调用栈在 MapPoint::position()

可能根因：

LocalMapping 删除 MapPoint。
Tracking 线程仍持有裸指针。
没有锁或生命周期协议。

最小复现：

TEST(MapConcurrencyTest, ReaderDoesNotUseDeletedPoint) {
    Map map;
    auto point_id = map.addPoint(Eigen::Vector3d(1, 2, 3));

    std::atomic<bool> stop{false};

    std::thread reader([&] {
        while (!stop.load(std::memory_order_acquire)) {
            // tryGetPoint 应返回受生命周期保护的句柄，而不是裸指针。
            auto point = map.tryGetPoint(point_id);
            if (point) {
                benchmark::DoNotOptimize(point->position());
            }
        }
    });

    // 删除与读取并发发生，用来逼出生命周期协议中的漏洞。
    map.removePoint(point_id);
    stop.store(true, std::memory_order_release);
    reader.join();
}

如果这个测试在 TSan 下报数据竞争，说明同步协议确实不完整。

修复后，这个测试进入 CI。以后同类问题会在提交阶段暴露，而不是在长时间运行后暴露。

示例：雅可比符号错误¶

现象：

位姿图在小扰动下收敛
初值稍差就发散

根因：

左扰动雅可比推导正确
代码实际使用右扰动更新

固化：

写有限差分雅可比测试。
测多个非零旋转状态。
在注释中明确扰动方向。
在因子接口命名中写出 leftPerturbation 或 rightPerturbation。

33.14 Mini 项目：建立一个 SLAM 核心库质量骨架 ⭐⭐⭐¶

目标¶

为一个小型 SLAM 核心库建立质量骨架：

mini_slam/
  CMakeLists.txt
  include/
    mini_slam/pose.hpp
    mini_slam/reprojection.hpp
    mini_slam/spsc_queue.hpp
  src/
    pose.cpp
    reprojection.cpp
  tests/
    test_pose.cpp
    test_reprojection.cpp
    test_spsc_queue.cpp
  benchmarks/
    bm_voxel_filter.cpp

CMake 结构¶

cmake_minimum_required(VERSION 3.22)
project(mini_slam_quality LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

option(ENABLE_ASAN "Enable AddressSanitizer" OFF)
option(ENABLE_UBSAN "Enable UndefinedBehaviorSanitizer" OFF)
option(ENABLE_TSAN "Enable ThreadSanitizer" OFF)

add_library(mini_slam
    src/pose.cpp
    src/reprojection.cpp
)

target_include_directories(mini_slam PUBLIC include)

target_compile_options(mini_slam PRIVATE
    -Wall -Wextra -Wpedantic -Wconversion -Wshadow
)

if(ENABLE_ASAN)
    target_compile_options(mini_slam PRIVATE -fsanitize=address -fno-omit-frame-pointer)
    target_link_options(mini_slam PRIVATE -fsanitize=address)
endif()

if(ENABLE_UBSAN)
    target_compile_options(mini_slam PRIVATE -fsanitize=undefined)
    target_link_options(mini_slam PRIVATE -fsanitize=undefined)
endif()

if(ENABLE_TSAN)
    target_compile_options(mini_slam PRIVATE -fsanitize=thread)
    target_link_options(mini_slam PRIVATE -fsanitize=thread)
endif()

enable_testing()
add_subdirectory(tests)

测试目录¶

find_package(GTest REQUIRED)

add_executable(test_pose
    test_pose.cpp
)

target_link_libraries(test_pose
    PRIVATE mini_slam GTest::gtest_main
)

add_test(NAME test_pose COMMAND test_pose)

必须覆盖的不变量¶

文件	测试目标
`test_pose.cpp`	旋转正交、组合逆、扰动一致
`test_reprojection.cpp`	投影中心点、负深度拒绝、雅可比有限差分
`test_spsc_queue.cpp`	顺序保持、满队列策略、关闭语义
`bm_voxel_filter.cpp`	不同点数下耗时趋势

本地运行顺序¶

cmake -S . -B build -DCMAKE_BUILD_TYPE=RelWithDebInfo
cmake --build build -j
ctest --test-dir build --output-on-failure

ASan：

cmake -S . -B build-asan -DENABLE_ASAN=ON
cmake --build build-asan -j
ctest --test-dir build-asan --output-on-failure

TSan：

cmake -S . -B build-tsan -DENABLE_TSAN=ON
cmake --build build-tsan -j
ctest --test-dir build-tsan --output-on-failure

33.15 🔧 故障排查手册 ⭐¶

现象	可能原因	优先工具	后续固化
偶发段错误	越界或 UAF	ASan + GDB	最小单元测试
运行数小时后崩溃	生命周期或数据竞争	TSan + trace	并发回归测试
Release 错 Debug 对	未定义行为或未初始化	UBSan + 编译警告	静态分析规则
优化器发散	雅可比或残差错误	有限差分测试	因子单元测试
轨迹轻微漂移	外参、时间、坐标系	数据集回归	配置验证测试
性能突然变差	分配、拷贝、锁竞争	perf + benchmark	性能阈值
CI 偶发失败	测试依赖时序	固定随机种子	消除非确定性
日志过多导致卡顿	同步日志在热路径	trace + profile	日志降级策略

33.16 练习与跨章综合题 ⭐⭐¶

为 C++语言核心/Eigen基础与SLAM数学预备中的 SE(3) 位姿组合写一个 GoogleTest：同时验证单位元、逆元和组合误差。
为并发与系统编程/实时约束与高性能数据传递中的 SPSC 队列写一个并发测试：生产者发送递增序列，消费者验证顺序和数量。
为一个 loadConfig() 函数设计异常测试：缺字段、类型错误、范围错误分别给出不同错误消息。
用 Google Benchmark 比较“每帧新建 vector”和“复用 workspace”两种点云过滤写法，并解释计时结果中是否包含分配成本。
跨章综合题：选择并发与系统编程/内存分配策略与pmr 的 FrameWorkspace 或并发与系统编程/CUDA在SLAM中的应用的 GPU fallback 接口，为它设计一组 CI 门禁。要求至少包含单元测试、Sanitizer 运行方式、性能阈值和一次故障复盘转成的回归测试。

这些练习的重点不是把测试数量堆多，而是让每个测试都对应一个明确不变量。

33.17 延伸阅读 ⭐¶

主题	资源	难度
GoogleTest	https://google.github.io/googletest/	⭐⭐
Google Benchmark	https://github.com/google/benchmark	⭐⭐
Catch2	https://github.com/catchorg/Catch2	⭐⭐
libFuzzer	https://llvm.org/docs/LibFuzzer.html	⭐⭐⭐
AddressSanitizer	https://clang.llvm.org/docs/AddressSanitizer.html	⭐⭐
ThreadSanitizer	https://clang.llvm.org/docs/ThreadSanitizer.html	⭐⭐
clang-tidy	https://clang.llvm.org/extra/clang-tidy/	⭐⭐
perf	https://perf.wiki.kernel.org	⭐⭐⭐
Valgrind/Callgrind	https://valgrind.org/docs/manual/cl-manual.html	⭐⭐⭐
Working Effectively with Legacy Code (Feathers)	测试遗留代码的经典方法	⭐⭐⭐

33.18 本章小结 ⭐¶

本章把代码质量工具放回机器人系统的语境中理解。它们不是互相替代，而是分层合作：

编译器警告和静态分析把错误前移到编译期。
GoogleTest 固定局部数学和资源不变量。
有限差分保护优化代码中的雅可比。
异常测试覆盖最少运行但最危险的错误路径。
Google Benchmark 提供可解释的性能证据。
Sanitizer 把隐蔽未定义行为变成明确错误。
GDB/LLDB 帮助从崩溃点回到根因。
perf、Callgrind 和 trace 找到真实热点。
CI 把个人习惯固化成团队规则。
故障复盘把一次事故转化成长期测试资产。

如果只记住一句话：

代码质量不是最后检查出来的，而是在每一层不变量中设计出来的。

下一章会进入日志、配置与序列化。那里关注的是另一条质量线：当系统运行在真实机器人上时，如何让参数、日志、地图文件和诊断信息可追踪、可复现、可演进。

代码质量、测试与调试¶

本章学习目标¶

知识树¶

前置自测¶

章节依赖¶

33.1 为什么机器人项目比普通业务代码更需要质量工具 ⭐⭐¶

工程问题：机器人 bug 往往不是立刻崩溃¶

反面失败：只用真实数据集测试¶

抽象不变量：每一层测试都守住一个不变量¶

⚠️ 常见陷阱¶

练习¶

33.2 测试金字塔：从函数到数据集 ⭐⭐¶

工程问题：为什么不能只做端到端测试¶

层级对比¶

反面失败：测试倒金字塔¶

设计原则¶

33.3 GoogleTest：从断言开始建立数学不变量 ⭐⭐¶

工程问题：SLAM 中最常见的错误是“差一点”¶

最小测试结构¶

浮点比较¶

旋转矩阵测试¶

四元数测试¶

反面失败：只测一个输入¶

⚠️ 常见陷阱¶

练习¶

33.4 Fixture：把测试初始化变成清晰对象 ⭐⭐¶

工程问题：测试准备代码重复后会漂移¶

基本写法¶

Fixture 不应该隐藏测试意图¶

参数化测试¶

⚠️ 常见陷阱¶

练习¶

33.5 雅可比测试：用有限差分保护优化代码 ⭐⭐⭐¶

工程问题：雅可比错一列，优化器可能仍然跑¶

有限差分思想¶

测试框架¶

易错点：扰动空间不是总在欧氏空间¶

⚠️ 常见陷阱¶

练习¶

33.6 异常与错误路径测试 ⭐⭐¶

过渡¶

工程问题：错误路径最少运行，却最容易出事¶

异常测试¶

资源不变量测试¶

死亡测试¶

33.7 Google Benchmark：性能测试不是打印时间 ⭐⭐¶

工程问题：std::chrono 手写计时很容易测错¶

最小 benchmark¶

反面失败：把准备数据算进核心算法¶

分配次数也是性能指标¶

解释 benchmark 结果¶

33.7b 属性测试与 Fuzzing：超越手写用例 ⭐⭐⭐¶

动机：手写测试只覆盖你想到的输入¶

如果不做属性测试会怎样¶

libFuzzer：让编译器帮你找崩溃¶

CI/CD 集成：把质量要求变成自动化流水线¶

⚠️ 常见陷阱¶

练习¶

33.8 Sanitizer：让未定义行为尽早爆炸 ⭐⭐¶

工程问题：C++ 错误不一定立刻报错¶

ASan：内存越界和释放后使用¶

UBSan：未定义行为¶

TSan：数据竞争¶

Sanitizer 组合边界¶

33.9 GDB/LLDB：从崩溃点回到原因 ⭐⭐¶

工程问题：崩溃位置不等于错误位置¶

基本命令¶

多线程调试¶

Watchpoint：找是谁改坏了变量¶

调试器与 Sanitizer 的关系¶

⚠️ 常见陷阱¶

练习¶

33.10 静态分析：把风格问题和潜在错误前移 ⭐⭐¶

工程问题：代码还没运行，很多错误已经能看出来¶

编译器警告¶

clang-tidy¶

反面失败：工具结果不分层¶

⚠️ 常见陷阱¶

练习¶

33.11 性能分析：先找热点，再谈优化 ⭐⭐¶

工程问题：`std::chrono` 手写计时很容易测错¶