文件 I/O 与字符串处理¶

难度：⭐⭐～⭐⭐⭐ | 建议用时：1周 | 前置要求：C++语言核心/RAII与智能指针 RAII与智能指针、C++语言核心/现代类设计与特殊成员函数现代类设计

前置自测¶

📋 答不出 ≥ 2 题 → 先回顾 C++语言核心/RAII与智能指针

RAII 的核心契约是什么？为什么 std::ifstream 不需要手动调用 close()？
reinterpret_cast 和 static_cast 有什么区别？什么场景下必须用前者？
std::string 的 c_str() 返回的指针在什么情况下会失效？
C++17 引入了哪些与文件系统相关的标准库设施？
什么是 endianness？x86 架构是 little-endian 还是 big-endian？

本章目标¶

学完本章，你将能够：

使用 std::ifstream/std::ofstream 正确读写 KITTI/TUM 数据集格式的轨迹文件，并通过 evo 工具链验证输出正确性
掌握 std::stringstream 的流式解析能力，能够解析 g2o 图文件等复杂文本格式
理解 std::string 的常用操作和 C++17 std::string_view 的零拷贝语义，在高频解析场景中选择正确的类型
用二进制 I/O 读写 PCL PCD 点云文件，理解为什么二进制格式比文本格式快 10-100 倍
使用 fmt 库（及 C++20 std::format）替代 iostream 实现高性能格式化输出
用 std::filesystem 遍历数据集目录、拼接路径、检查文件存在性
了解 SLAM 系统常用的序列化框架（Boost.Serialization、Protobuf、nlohmann/json、cereal）的适用场景与性能权衡

本章在课程中的位置：文件 I/O 贯穿 SLAM 全流程。从第 22 章开始，你将使用 Eigen、Sophus、PCL、g2o 等核心库——它们的输入输出全部依赖本章内容。KITTI/TUM 数据集是 SLAM 评测的标准数据源，不掌握其文件格式就无法运行任何 benchmark。本章也是 C++语言核心/RAII与智能指针 RAII 原则的第一个大规模工程应用：ifstream/ofstream 的析构自动关闭文件，正是 RAII 在标准库中的经典体现。

知识树¶

文件 I/O 与字符串处理
├── fstream 基础（21.1）
│   ├── RAII 保证
│   └── 三种读取模式：逐行 / operator>> / 二进制
├── KITTI/TUM 数据集格式（21.2）
│   └── 标准化评测的基础
├── stringstream（21.3）
│   └── 类型安全的字符串解析
├── string 与 string_view（21.4）
│   └── 零拷贝的字符串引用（C++17）
├── g2o 图文件格式（21.5）
│   └── 图优化数据 I/O
├── 二进制文件读写（21.6）
│   ├── reinterpret_cast 与 POD
│   └── 内存映射文件（mmap）
├── fmt 与 std::format（21.7）
│   ├── 编译期类型安全
│   └── C++20 std::format / C++23 std::print
├── std::filesystem（21.8）
│   └── 跨平台路径操作（C++17）
└── 序列化框架概览（21.9）
    ├── Boost.Serialization / Protobuf
    └── nlohmann/json / cereal

21.1 std::ifstream/std::ofstream 基础 ⭐⭐¶

动机：SLAM 系统离不开文件读写¶

SLAM 系统在运行的每一个阶段都在和文件打交道。启动时，它需要从磁盘加载相机内参、IMU 标定参数、词汇树文件；运行时，它逐帧读取图像和点云数据；结束时，它需要把估计的轨迹保存为标准格式供评测工具分析。一个典型的 SLAM pipeline 涉及的文件 I/O 操作包括：

阶段	文件类型	读/写	示例
初始化	YAML 配置	读	ORB-SLAM3 的 `EuRoC.yaml`
初始化	词汇树文件	读	ORB-SLAM3 的 `ORBvoc.txt`（139MB）
数据加载	时间戳文件	读	KITTI 的 `times.txt`
数据加载	点云二进制	读	KITTI 的 `.bin` 文件
运行中	日志文件	写	spdlog 输出到 `slam.log`
结束	轨迹文件	写	TUM 格式 `trajectory.txt`
结束	地图文件	写	ORB-SLAM3 的 Atlas 序列化

如果你不能正确地读写文件，后面所有 SLAM 库的使用都无从谈起。这就是为什么文件 I/O 被安排在 SLAM 核心库剖析的第一章。

如果不用 C++ 流会怎样：C 风格文件 I/O 的痛苦¶

在 C++语言核心/RAII与智能指针中我们已经看到，C 风格的 fopen/fclose 需要手动配对，任何一条 return 路径上忘了 fclose 就是资源泄漏。但 C 风格 I/O 的问题远不止于此。

// C 风格读取 TUM 轨迹——每一步都可能出错
void loadTUM_C(const char* filename) {
    FILE* fp = fopen(filename, "r");
    if (!fp) {
        fprintf(stderr, "Cannot open %s\n", filename);
        return;
    }
    char line[512];
    while (fgets(line, sizeof(line), fp)) {
        double ts, tx, ty, tz, qx, qy, qz, qw;
        int n = sscanf(line, "%lf %lf %lf %lf %lf %lf %lf %lf",
                        &ts, &tx, &ty, &tz, &qx, &qy, &qz, &qw);
        if (n != 8) continue;
    }
    fclose(fp);  // 忘了这行？泄漏 fd。异常路径中途 return？也泄漏
}

这段代码有三个工程问题。第一，fclose 必须手动调用，而且必须在每条退出路径上都调用——如果后续在循环内部加了 return（比如检测到坏数据提前退出），文件描述符就泄漏了。第二，fgets 的固定缓冲区大小 512 字节是一个隐患——如果某行超过 512 字节（比如包含很长的注释），数据会被截断，sscanf 解析出错但不报错，产生难以追踪的 bug。第三，sscanf 的 %lf 格式符必须和变量类型完全匹配——写错了（比如用 %f 读 double）不报编译错误，运行时直接产生垃圾值。

C++ 文件流的 RAII 保证¶

C++ 的 <fstream> 提供了 std::ifstream（输入文件流）和 std::ofstream（输出文件流），它们是 RAII 资源类型——构造函数打开文件，析构函数自动关闭文件。这直接消除了 C 风格 I/O 的第一个问题。

#include <fstream>
#include <string>
#include <iostream>

void loadTUM_Cpp(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        std::cerr << "Cannot open " << filename << "\n";
        return;
    }
    std::string line;
    while (std::getline(ifs, line)) {
        // line 的长度自动扩展，不存在缓冲区溢出
        // 后面用 stringstream 解析（21.3 节详述）
    }
    // ifs 在离开作用域时自动调用 close()——RAII 保证
}

为什么不需要手动调用 ifs.close()？ 回顾 C++语言核心/RAII与智能指针的 RAII 核心契约：析构函数释放资源。std::ifstream 的析构函数内部调用了 close()，而析构函数由编译器在对象离开作用域时自动调用——无论是正常退出、提前 return、还是异常抛出。你可以写 ifs.close() 来显式关闭，但这通常是不必要的——除非你需要在同一个 ifstream 对象上先关闭当前文件、再打开另一个文件。

is_open() vs operator bool() 的区别。ifs.is_open() 只检查文件是否成功打开，不检查后续读取状态。if (ifs) 或等价的 if (!ifs.fail()) 则同时检查 failbit 和 badbit。在打开文件后立即检查时两者等价，但在读取过程中，应使用 operator bool() 检查流状态。

三种读取模式的对比¶

C++ 文件流提供三种主要的读取方式，适用于不同场景：

模式 A：逐行读取 + 后续解析

这是最常用的模式，适合每行格式可能不同的文件（如 g2o 文件，每行以不同的关键字开头）。

#include <fstream>
#include <string>
#include <sstream>
#include <vector>

std::vector<double> readTimestamps(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open: " + filename);
    }

    std::vector<double> timestamps;
    std::string line;
    while (std::getline(ifs, line)) {
        if (line.empty() || line[0] == '#') continue;
        timestamps.push_back(std::stod(line));
    }
    return timestamps;
}

模式 B：operator>> 格式化读取

适合每行格式固定、字段之间以空白分隔的文件。operator>> 自动跳过空白字符（空格、制表符、换行符）。

#include <fstream>
#include <vector>
#include <array>

struct Pose12 {
    std::array<double, 12> data;  // 3x4 变换矩阵，行优先
};

std::vector<Pose12> readKITTIPoses(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open: " + filename);
    }

    std::vector<Pose12> poses;
    Pose12 p;
    while (ifs >> p.data[0]) {
        for (int i = 1; i < 12; ++i) {
            ifs >> p.data[i];
        }
        poses.push_back(p);
    }
    return poses;
}

模式 C：二进制读取

适合大规模数据（点云、深度图），21.6 节详述。

#include <fstream>
#include <vector>

std::vector<float> readBinPointCloud(const std::string& filename) {
    std::ifstream ifs(filename, std::ios::binary | std::ios::ate);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open: " + filename);
    }
    auto size = ifs.tellg();
    ifs.seekg(0, std::ios::beg);

    std::vector<float> data(size / sizeof(float));
    ifs.read(reinterpret_cast<char*>(data.data()), size);
    return data;
}

模式	适用场景	优点	缺点
`getline` + 后续解析	行格式不固定	灵活，可处理注释行	需要额外解析步骤
`operator>>`	行格式固定、空白分隔	代码简洁	跳过换行符，丢失行结构
`read` 二进制	大规模数值数据	极快，无解析开销	需要知道数据布局

格式化输出与精度控制¶

将轨迹保存为文件时，精度控制至关重要。SLAM 系统的时间戳通常是 Unix 时间（如 1317384018.684439），需要至少 6 位小数精度。如果使用默认的 cout 格式，double 只显示 6 位有效数字，时间戳会被截断为 1.31738e+09——所有小数部分丢失。

#include <fstream>
#include <iomanip>
#include <vector>

struct TUMPose {
    double timestamp;
    double tx, ty, tz;
    double qx, qy, qz, qw;
};

void saveTUM(const std::string& filename,
             const std::vector<TUMPose>& trajectory) {
    std::ofstream ofs(filename);
    if (!ofs.is_open()) {
        throw std::runtime_error("Cannot create: " + filename);
    }

    ofs << std::fixed << std::setprecision(9);
    for (const auto& pose : trajectory) {
        ofs << pose.timestamp << " "
            << pose.tx << " " << pose.ty << " " << pose.tz << " "
            << pose.qx << " " << pose.qy << " "
            << pose.qz << " " << pose.qw << "\n";
    }
}

为什么用 std::fixed 而不是默认格式？ 默认的 defaultfloat 格式以"最短表示"输出——对 1317384018.684439 这样的大数，它会用科学计数法 1.31738e+09，丢失小数位。std::fixed 强制使用定点表示，setprecision(9) 指定小数位数为 9，确保 double 能表示的所有有效位都被输出不会丢失。注意：对于 ~10 位整数部分的 Unix 时间戳，double 的 52 位尾数只能提供 ~6 位有效小数位，所以第 7-9 位小数本质上是浮点噪声，并非真正的纳秒精度。

为什么用 "\n" 而不是 std::endl？ std::endl 等价于 "\n" + flush()。每次 flush() 都会强制将缓冲区写入磁盘，对于逐行写入数千行的轨迹文件，这意味着数千次系统调用。使用 "\n" 让标准库自己管理缓冲（通常 8KB 一次），写入速度可以快 10 倍以上。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：用 ifs >> value 读取后紧接 getline，导致读到空行

错误做法：先用 >> 读数值，再用 getline 读下一行。

现象：getline 返回空字符串，好像"吃掉"了一行数据。实际上你的循环少处理了一行。

根本原因：operator>> 读取数值后，会停在分隔符（空格或换行符 \n）之前。随后的 getline 读取到这个残留的 \n 就立即返回空字符串。这是 C++ 流最经典的坑之一。

正确做法：在 >> 和 getline 之间加 ifs.ignore(std::numeric_limits<std::streamsize>::max(), '\n'); 丢弃残留的换行符。或者统一用 getline + stringstream 解析每一行。

⚠️ 编程陷阱：检测文件结束时用 while (!ifs.eof()) 导致多读一次

错误做法：
while (!ifs.eof()) {
    ifs >> value;
    process(value);
}
现象：最后一个值被处理了两次。

根本原因：eof() 只在**已经尝试读取越过文件末尾**之后才返回 true。在最后一次成功读取后，eof() 仍为 false，循环再执行一次，ifs >> value 失败，但 value 保留了上次的值，process(value) 重复处理。

正确做法：用读取操作本身作为循环条件——while (ifs >> value) 或 while (std::getline(ifs, line))。读取失败时流的 operator bool() 返回 false，循环自动终止。

💡 概念误区：认为 ifstream 在 debug 模式下更慢

新手想法："fstream 是 C++ 的高层封装，肯定比 C 的 fread 慢很多。"

实际上：在现代编译器（GCC/Clang -O2）下，ifstream::read 和 fread 的性能几乎相同——它们最终都调用相同的系统调用（read(2)）。性能差异主要来自文本解析（>> 运算符的 locale 处理开销），而非 I/O 本身。如果需要极致文本解析性能，可以关闭 sync_with_stdio 或使用 mmap。

正确理解：fstream 的"慢"来自两个默认行为：(1) std::ios::sync_with_stdio(true) 让 cin/cout 与 scanf/printf 同步，有锁开销；(2) locale 处理让 >> 需要检查千分位分隔符等设置。对于文件流（而非 cin），这些通常不影响实际性能。

练习¶

实践题：编写一个函数 countLines(const std::string& filename)，统计文件的行数（不含空行和以 # 开头的注释行）。用 KITTI 的 times.txt 测试——sequence 00 应该有 4541 行。
对比题：分别用 operator>> 和 getline + stod 两种方式读取一列浮点数文件。测量两者在 10 万行数据上的耗时差异。解释为什么 operator>> 可能更慢（提示：locale）。
设计题：设计一个 ScopedFile 类，在构造时接受文件名和模式（"r" / "w" / "rb"），内部用 FILE* 实现，析构时自动 fclose。讨论：为什么标准库已经提供了 fstream，但某些 SLAM 代码（如 ORB-SLAM3 的词汇树加载）仍然使用 C 风格 I/O？

21.2 KITTI/TUM 数据集格式 ⭐⭐¶

动机：标准化的评测数据是 SLAM 研究的基础¶

SLAM 算法的好坏需要在标准数据集上用定量指标衡量。如果每个研究组用自己的私有数据、自己定义的格式和指标，论文之间的结果就无法横向对比。KITTI（Karlsruhe Institute of Technology and Toyota Technological Institute）和 TUM RGB-D 数据集通过定义标准的文件格式和评测工具，让全世界的 SLAM 研究者可以在同一把尺子下比较算法性能。

所有主流 SLAM 评测工具（evo_ape、evo_rpe、KITTI 官方 devkit）都读取这两种格式。你的 SLAM 系统输出的轨迹文件如果不符合格式规范，评测工具会报错或产生错误结果。

KITTI 格式详解¶

KITTI 数据集的核心文件有两个：

times.txt——时间戳文件

每行一个时间戳（单位：秒），从 0 开始递增。行号对应帧号。

0.000000e+00
1.106519e-01
2.213038e-01
3.319557e-01
4.426077e-01

注意时间戳使用科学计数法表示。std::stod 和 operator>> 都能正确解析这种格式。

poses.txt——轨迹文件（仅 ground truth 提供）

每行 12 个空格分隔的浮点数，代表一个 3x4 变换矩阵 \(T_{wc}\)（相机坐标系到世界坐标系的变换，即相机在世界坐标系下的位姿），按行优先存储：

\[T_{wc} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \end{bmatrix}\]

文件中每行的 12 个数按 \(r_{11}, r_{12}, r_{13}, t_x, r_{21}, r_{22}, r_{23}, t_y, r_{31}, r_{32}, r_{33}, t_z\) 排列。第四行 \([0, 0, 0, 1]\) 被省略（因为刚体变换（SE(3)）的齐次表示中，最后一行恒为 \([0, 0, 0, 1]\)）。

1.000000e+00 9.043680e-12 2.326809e-11 5.551115e-17 9.043683e-12 1.000000e+00 2.392370e-10 3.330669e-16 2.326810e-11 2.392370e-10 9.999999e-01 -1.110223e-16
9.999978e-01 5.272628e-04 -2.066935e-03 -4.690294e-02 -5.296506e-04 9.999992e-01 -1.154865e-03 -2.839928e-02 2.066324e-03 1.155958e-03 9.999971e-01 8.586941e-01

读取 KITTI poses 的完整代码：

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <array>
#include <stdexcept>

struct KITTIPose {
    std::array<double, 12> data;

    double operator()(int row, int col) const {
        return data[row * 4 + col];
    }
};

std::vector<KITTIPose> loadKITTIPoses(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open KITTI poses: " + filename);
    }

    std::vector<KITTIPose> poses;
    std::string line;
    int line_num = 0;

    while (std::getline(ifs, line)) {
        ++line_num;
        if (line.empty()) continue;

        std::istringstream iss(line);
        KITTIPose pose;
        for (int i = 0; i < 12; ++i) {
            if (!(iss >> pose.data[i])) {
                throw std::runtime_error(
                    "Parse error at line " + std::to_string(line_num) +
                    ": expected 12 values");
            }
        }
        poses.push_back(pose);
    }
    return poses;
}

TUM 格式详解¶

TUM RGB-D 数据集使用一种更紧凑的轨迹格式。每行 8 个值：

timestamp tx ty tz qx qy qz qw

其中 (tx, ty, tz) 是平移向量，(qx, qy, qz, qw) 是表示旋转的单位四元数。一个典型的 TUM 轨迹文件片段：

1305031102.175304 -0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 1.0000
1305031102.211214 0.0003 -0.0024 0.0003 -0.0008 0.0014 0.0001 1.0000
1305031102.243195 0.0010 -0.0058 0.0009 -0.0016 0.0022 -0.0001 1.0000

TUM 格式中四元数的分量顺序与 Eigen 完全一致。 Eigen 的 Quaterniond 内部存储顺序为 [x, y, z, w]（通过 coeffs() 方法可以验证），与 TUM 格式的 qx qy qz qw 一一对应。但要注意 Eigen 的构造函数参数顺序是 Quaterniond(w, x, y, z)——w 在前。这是两个不同的概念：构造函数参数顺序 vs 内存存储顺序。

读取和保存 TUM 格式的完整代码：

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <stdexcept>
#include <iomanip>
#include <cmath>

struct TUMPose {
    double timestamp;
    double tx, ty, tz;
    double qx, qy, qz, qw;
};

std::vector<TUMPose> loadTUM(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open TUM file: " + filename);
    }

    std::vector<TUMPose> trajectory;
    std::string line;

    while (std::getline(ifs, line)) {
        if (line.empty() || line[0] == '#') continue;

        std::istringstream iss(line);
        TUMPose pose;
        if (!(iss >> pose.timestamp
                  >> pose.tx >> pose.ty >> pose.tz
                  >> pose.qx >> pose.qy >> pose.qz >> pose.qw)) {
            continue;
        }

        double norm = std::sqrt(pose.qx * pose.qx + pose.qy * pose.qy +
                                pose.qz * pose.qz + pose.qw * pose.qw);
        if (std::abs(norm - 1.0) > 1e-3) {
            throw std::runtime_error("Non-unit quaternion at t=" +
                                      std::to_string(pose.timestamp));
        }

        trajectory.push_back(pose);
    }
    return trajectory;
}

void saveTUM(const std::string& filename,
             const std::vector<TUMPose>& trajectory) {
    std::ofstream ofs(filename);
    if (!ofs.is_open()) {
        throw std::runtime_error("Cannot create TUM file: " + filename);
    }

    ofs << std::fixed << std::setprecision(9);
    for (const auto& p : trajectory) {
        ofs << p.timestamp << " "
            << p.tx << " " << p.ty << " " << p.tz << " "
            << p.qx << " " << p.qy << " " << p.qz << " " << p.qw << "\n";
    }
}

KITTI 与 TUM 格式对比¶

特性	KITTI poses.txt	TUM format
旋转表示	3x3 旋转矩阵（9 个数）	单位四元数（4 个数）
平移表示	矩阵最后一列（3 个数）	显式 tx ty tz
时间戳	独立文件 `times.txt`	每行第一个字段
每行字段数	12	8
是否包含时间戳	否（靠行号对应）	是
主要使用者	自动驾驶（车载激光雷达）	室内 RGB-D
评测工具	KITTI devkit, evo	evo, TUM 官方工具

evo 评测工具的使用¶

evo 是 SLAM 社区最常用的轨迹评测工具。它直接读取 TUM 和 KITTI 格式的文件，计算 APE（Absolute Pose Error）和 RPE（Relative Pose Error）。

确保你的轨迹文件格式正确后，可以直接在命令行使用：

# 计算绝对轨迹误差（APE）——TUM 格式
evo_ape tum groundtruth.txt estimated.txt -p --plot_mode=xz

# 计算相对位姿误差（RPE）——KITTI 格式
evo_rpe kitti gt_poses.txt est_poses.txt -p

如果 evo 报格式错误，最常见的原因是：时间戳精度不够（被截断为科学计数法）、行尾有多余空格、四元数未归一化。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：KITTI poses 的 12 个数顺序搞错——行优先 vs 列优先

错误做法：把 12 个数按列优先填入 4x4 矩阵。

现象：轨迹显示为镜像或完全扭曲的形状，但不报任何错误。

根本原因：KITTI 的 poses.txt 使用行优先（row-major）存储——前 4 个数是矩阵第一行 [r11, r12, r13, tx]，不是第一列。Eigen 默认也是列优先（column-major）存储，所以直接把 12 个数 memcpy 进 Eigen::Matrix4d 会产生转置效果。

正确做法：读取后逐元素赋值，或使用 Eigen::Map 配合 Eigen::RowMajor 标志：
Eigen::Matrix<double, 3, 4, Eigen::RowMajor> T_row;
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(T_row.data()) =
    Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(pose.data.data());
⚠️ 编程陷阱：Eigen 四元数构造函数的参数顺序 vs 内部存储顺序混淆

错误做法：直接将 TUM 文件中的 qx qy qz qw 顺序传给 Quaterniond 构造函数。

现象：旋转结果错误——x 分量变成了 w，w 变成了 z。

根本原因：Eigen::Quaterniond 的构造函数签名是 Quaterniond(w, x, y, z)——w 排在最前面。但内部 coeffs() 返回的顺序是 [x, y, z, w]。TUM 文件中的顺序是 qx qy qz qw，与 coeffs() 一致，但与构造函数相反。

正确做法：
// 从 TUM 文件读取后：
Eigen::Quaterniond q(qw, qx, qy, qz);  // 构造函数：w 在前
// 或者用 coeffs 赋值：
Eigen::Quaterniond q;
q.coeffs() << qx, qy, qz, qw;  // coeffs 顺序：x,y,z,w
💡 概念误区：认为 TUM 格式的时间戳是相对时间

新手想法："TUM 的时间戳 1305031102.175304 太大了，应该是哪里搞错了。"

实际上：TUM 时间戳是 Unix epoch 时间——从 1970-01-01 00:00:00 UTC 开始的秒数。1305031102 对应 2011 年 5 月 10 日。KITTI 的时间戳则是从序列开始的相对时间（从 0 开始）。两者的时间戳含义完全不同。

为什么重要：计算两帧间时间差（dt = t2 - t1）时，double 的有效数字约 15-16 位。Unix 时间戳本身就占了 10 位整数 + 6 位小数 = 16 位，接近 double 的精度极限。因此保存时必须用 std::fixed << std::setprecision(9) 确保不丢失有效位。

练习¶

实践题：下载 KITTI sequence 00 的 ground truth（poses.txt 和 times.txt），用本节代码读取后转换为 TUM 格式保存。用 evo_traj tum your_output.txt -p 可视化轨迹，确认形状正确。
分析题：KITTI 的 poses.txt 中没有时间戳信息，为什么 evo 工具仍然能计算 RPE？提示：RPE 可以按帧间距计算（每隔 N 帧），不一定需要时间信息。

21.3 std::stringstream ⭐⭐¶

动机：从字符串中提取结构化数据¶

在 21.1 节中我们用 getline 逐行读取文件，但 getline 只给我们一个完整的字符串——还需要从中提取出各个字段。C 语言用 sscanf 做这件事，但 sscanf 有类型不安全的问题（格式符和变量类型不匹配不报编译错误）。C++ 的 std::stringstream 把字符串当作流来处理，复用 operator>> 的类型安全解析能力。

如果不用 stringstream 会怎样¶

手动解析字符串意味着自己处理分隔符、空格、类型转换——既繁琐又容易出错。

// 手动解析 TUM 行——脆弱且容易出错
void parseLine_manual(const std::string& line) {
    size_t pos = 0;
    size_t next;

    next = line.find(' ', pos);
    double timestamp = std::stod(line.substr(pos, next - pos));
    pos = next + 1;

    next = line.find(' ', pos);
    double tx = std::stod(line.substr(pos, next - pos));
    pos = next + 1;

    // 还要继续写 6 次...每多一个字段就多 3 行代码
    // 如果字段之间有多个空格或制表符？这段代码直接崩溃
}

这种方式的问题在于：每多一个字段就要重复相似的代码，无法处理不同的空白字符（制表符、多个空格），而且 find 返回 npos 时的边界条件容易遗漏。

stringstream 的核心机制¶

类比：std::istringstream 就像把一个字符串变成了一条"虚拟文件流"。你可以用和 ifstream 完全相同的 >> 操作符从中读取数据——区别只是数据来源是内存中的字符串而非磁盘上的文件。这种统一的流接口设计是 C++ I/O 库的核心思想：无论数据来自文件、字符串还是网络，处理方式都一样。

std::istringstream 将一个 std::string 当作输入流。你可以像从 cin 或 ifstream 中读取一样，用 >> 从字符串中提取数据。

#include <sstream>
#include <string>
#include <iostream>

void parseTUMLine(const std::string& line) {
    std::istringstream iss(line);
    double timestamp, tx, ty, tz, qx, qy, qz, qw;

    if (iss >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw) {
        std::cout << "Time: " << timestamp
                  << ", Position: (" << tx << ", " << ty << ", " << tz << ")\n";
    }
}

operator>> 自动跳过空白字符（空格、制表符、换行符），自动将文本转换为目标类型（double、int、std::string 等），失败时设置流的 failbit——只需检查 if (iss) 即可知道解析是否成功。

ostringstream 用于构建字符串。与 istringstream 相反，std::ostringstream 将输出"写"到一个字符串中：

#include <sstream>
#include <iomanip>
#include <string>

std::string formatTimedMessage(int frame_id, double timestamp) {
    std::ostringstream oss;
    oss << "Frame " << std::setw(6) << std::setfill('0') << frame_id
        << " at t=" << std::fixed << std::setprecision(6) << timestamp << "s";
    return oss.str();
}

stringstream 在 SLAM 文件解析中的典型用法¶

SLAM 中大量文件的每行格式由第一个字段（关键字）决定——g2o 文件中 VERTEX_SE2 开头的行和 EDGE_SE2 开头的行格式完全不同。这种"先读关键字再按格式解析"的模式用 stringstream 非常自然：

#include <fstream>
#include <sstream>
#include <string>
#include <iostream>

void parseG2OFile(const std::string& filename) {
    std::ifstream ifs(filename);
    std::string line;

    while (std::getline(ifs, line)) {
        if (line.empty() || line[0] == '#') continue;

        std::istringstream iss(line);
        std::string tag;
        iss >> tag;

        if (tag == "VERTEX_SE2") {
            int id;
            double x, y, theta;
            iss >> id >> x >> y >> theta;
            std::cout << "Vertex " << id << ": (" << x << ", " << y << ")\n";
        } else if (tag == "EDGE_SE2") {
            int id1, id2;
            double dx, dy, dtheta;
            iss >> id1 >> id2 >> dx >> dy >> dtheta;
            // 后面还有信息矩阵的上三角元素
            std::cout << "Edge " << id1 << " -> " << id2 << "\n";
        }
    }
}

stringstream 与其他解析方式的对比¶

方式	类型安全	处理多种空白	错误检测	性能
`sscanf`	否（格式符错不报编译错误）	是	通过返回值	快
`stringstream >>`	是（编译期类型检查）	是	通过流状态	中等
`stod` + `find`	是	否（需要自己处理）	通过异常	快
`std::from_chars` (C++17)	是	否	通过返回值	最快

std::from_chars 是 C++17 引入的高性能解析函数，不处理 locale，不抛异常，速度最快。但它不支持自动跳过空白，需要自己管理指针位置。对于要求极致性能的解析场景（如实时处理传感器数据），from_chars 是更好的选择；对于数据集离线解析，stringstream 的便利性更有价值。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：重复使用同一个 stringstream 而不 clear()

错误做法：
std::istringstream iss;
for (const auto& line : lines) {
    iss.str(line);       // 设置新内容
    iss >> value;        // 第二次迭代开始失败
}
现象：第一行解析正确，后续所有行都解析失败。

根本原因：str() 只替换了底层字符串，但没有重置流状态标志（eofbit、failbit）。第一次读到末尾后 eofbit 被置位，后续所有读取操作都会直接失败。

正确做法：在 str() 之后调用 clear() 重置状态标志：
iss.str(line);
iss.clear();  // 重置 eofbit/failbit
iss >> value;
🧠 思维陷阱：在性能关键路径上使用 stringstream

新手想法："stringstream 很方便，在实时处理循环中也用它解析传感器数据。"

实际上：stringstream 的构造和析构涉及动态内存分配（内部的 std::string 缓冲区），在每帧都要解析数据的场景下（如 100Hz IMU 数据），每帧创建/销毁一个 stringstream 意味着每秒 100 次 malloc/free。

正确做法：在实时路径上，使用 std::from_chars（C++17）直接从 const char* 解析，零分配，零 locale 开销。或者将 stringstream 定义在循环外面，每次用 str() + clear() 复用。

练习¶

实践题：编写函数 std::vector<std::string> split(const std::string& s, char delim)，用 getline(iss, token, delim) 实现。测试：split("a,b,,c", ',') 应返回 {"a", "b", "", "c"}——注意空字段。
对比题：用 stringstream 和 sscanf 分别实现解析 "12.5 3.14 -7.0" 为三个 double。测量 10 万次迭代的耗时差异。解释 stringstream 更慢的原因（动态分配 + locale）。

21.4 std::string 操作与 string_view ⭐⭐¶

动机：字符串是 SLAM 系统中的"胶水"¶

SLAM 系统中，字符串操作无处不在：拼接文件路径、解析配置文件的键值对、从传感器 topic 名称中提取传感器类型、生成带序号的文件名（如 000001.png）。虽然不是计算密集型操作，但如果使用不当——尤其是在循环中频繁创建临时字符串——可能成为意想不到的性能瓶颈。

std::string 常用操作速览¶

查找与定位：

#include <string>
#include <iostream>

void stringSearchDemo() {
    std::string path = "/data/kitti/sequences/00/velodyne/000001.bin";

    // find: 从左向右查找，返回首次出现的位置
    size_t pos = path.find("velodyne");  // pos = 28
    if (pos != std::string::npos) {
        std::cout << "Found 'velodyne' at position " << pos << "\n";
    }

    // rfind: 从右向左查找——找最后一个 '/'
    size_t last_slash = path.rfind('/');  // last_slash = 37

    // substr: 提取子串
    std::string filename = path.substr(last_slash + 1);  // "000001.bin"
    std::string dir = path.substr(0, last_slash);         // "/data/kitti/.../velodyne"

    // find_last_of: 查找字符集中任意字符最后出现的位置
    size_t dot = path.find_last_of('.');  // 找最后一个 '.'
    std::string ext = path.substr(dot);   // ".bin"
}

数值转换：

stoi/stod/stof 系列函数将字符串转为数值，to_string 反向转换。

#include <string>
#include <stdexcept>

void conversionDemo() {
    // 字符串 -> 数值
    int frame_id = std::stoi("000042");     // 42（忽略前导零）
    double timestamp = std::stod("1305031102.175304");
    float depth = std::stof("3.14");

    // stoi 的第二个参数可以获取解析了多少字符
    size_t pos;
    int val = std::stoi("123abc", &pos);  // val=123, pos=3

    // 数值 -> 字符串
    std::string s1 = std::to_string(42);       // "42"
    std::string s2 = std::to_string(3.14);     // "3.140000"（注意多余的零）
}

to_string 对浮点数总是输出 6 位小数，无法控制精度。如果需要精确控制输出格式，应使用 std::ostringstream 或 fmt::format（21.7 节）。

字符串修改：

#include <string>
#include <algorithm>

void modifyDemo() {
    std::string topic = "/camera/left/image_raw";

    // replace: 替换指定位置的子串
    std::string modified = topic;
    size_t pos = modified.find("left");
    if (pos != std::string::npos) {
        modified.replace(pos, 4, "right");  // "/camera/right/image_raw"
    }

    // erase: 删除子串
    std::string trimmed = "  hello  ";
    trimmed.erase(0, trimmed.find_first_not_of(' '));  // 去除前导空格
    trimmed.erase(trimmed.find_last_not_of(' ') + 1);  // 去除尾部空格

    // starts_with / ends_with (C++20)
    // C++17 中需要手写：
    bool is_bin = topic.size() >= 4 &&
                  topic.compare(topic.size() - 4, 4, ".bin") == 0;
}

C++17 std::string_view：零拷贝的字符串引用¶

std::string_view 是 C++17 引入的轻量级字符串"视图"。它不拥有字符串数据——只存储一个指针和一个长度。创建 string_view 不涉及内存分配，也不拷贝任何字符。

为什么需要 string_view？ 在解析文件的每一行时，如果你用 substr() 提取子串，每次 substr 都会创建一个新的 std::string 对象，分配堆内存，拷贝字符数据。对于高频解析场景（每秒处理上万行数据），这些临时分配会成为性能瓶颈。string_view 只是"指向原始字符串的一个窗口"，没有任何分配开销。

类比：std::string::substr 像是用复印机复印书中的一个段落——每次都要取纸、印刷、产出一份独立的副本。std::string_view::substr 像是用手指指着原书中的某个段落——零成本，但你不能修改书的内容，而且你必须确保书没被撤走。

本质洞察：string_view 的设计体现了 C++ 的一个核心原则——"不为不使用的功能付出代价"（zero-overhead principle）。如果你只需要读取一段字符串而不需要拥有它，就不应该为拥有权（内存分配和拷贝）付出代价。这和 const & 传参的思想一致：能不拷贝就不拷贝。

#include <string>
#include <string_view>

void stringViewDemo() {
    std::string line = "VERTEX_SE2 42 1.5 2.3 0.78";

    // string 的 substr: 分配新内存，拷贝字符
    std::string tag_copy = line.substr(0, 10);  // 堆分配 + 拷贝

    // string_view 的 substr: 零拷贝，只调整指针和长度
    std::string_view sv(line);
    std::string_view tag_view = sv.substr(0, 10);  // 无分配，无拷贝

    // string_view 支持大部分 const 操作
    size_t pos = sv.find(' ');
    std::string_view first_word = sv.substr(0, pos);

    // 但 string_view 不能修改底层数据
    // tag_view[0] = 'v';  // 编译错误
}

string_view 的核心规则：它不拥有数据，生命周期必须短于原始字符串。 如果原始字符串被销毁或修改，所有指向它的 string_view 都变成悬垂引用（dangling reference）——使用它们是未定义行为。

// ✅ 正确：string_view 在 line 的作用域内使用
std::string line = "hello world";
std::string_view sv = line;
std::cout << sv << "\n";  // OK

// ❌ 危险：返回 string_view 指向局部 string
std::string_view dangerousGetName() {
    std::string name = "SLAM";
    return name;  // name 在函数返回时销毁，string_view 悬垂！
}

string_view 在 SLAM 解析中的应用¶

解析配置文件的键值对时，string_view 可以避免大量临时 string 的创建：

#include <string>
#include <string_view>
#include <unordered_map>
#include <fstream>
#include <sstream>

std::unordered_map<std::string, std::string> parseConfig(
    const std::string& filename) {
    std::ifstream ifs(filename);
    std::unordered_map<std::string, std::string> config;
    std::string line;

    while (std::getline(ifs, line)) {
        std::string_view sv(line);

        // 跳过注释和空行
        if (sv.empty() || sv[0] == '#') continue;

        // 查找分隔符 '='
        auto eq_pos = sv.find('=');
        if (eq_pos == std::string_view::npos) continue;

        // 用 string_view 做零拷贝切分
        std::string_view key_sv = sv.substr(0, eq_pos);
        std::string_view val_sv = sv.substr(eq_pos + 1);

        // 去除前后空格（手动 trim）
        while (!key_sv.empty() && key_sv.back() == ' ') key_sv.remove_suffix(1);
        while (!val_sv.empty() && val_sv.front() == ' ') val_sv.remove_prefix(1);

        // 存入 map 时必须转换为 string（map 需要拥有数据）
        config[std::string(key_sv)] = std::string(val_sv);
    }
    return config;
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：从临时 string 创建 string_view 导致悬垂引用

错误做法：
std::string_view sv = std::string("hello");  // 临时 string 在语句结束时销毁
std::cout << sv;  // 未定义行为！
现象：可能输出正确内容、乱码、或段错误——取决于被销毁的内存是否被覆写。

根本原因：std::string("hello") 创建一个临时对象，string_view 记录了其内部缓冲区的指针。语句结束后临时对象析构，指针悬垂。

正确做法：确保 string_view 指向的字符串比 string_view 活得更久。字面量 "hello" 是静态存储的，可以安全使用：std::string_view sv = "hello";。

💡 概念误区：认为 stoi/stod 遇到非数字字符会默默返回 0

新手想法："stoi("abc") 返回 0 吧，就像 Python 的 int() 抛异常一样？等等，C++ 也抛异常？"

实际上：stoi("abc") 抛出 std::invalid_argument 异常。stoi("99999999999") 抛出 std::out_of_range 异常。如果你没有 try/catch，程序直接终止。

正确做法：在解析用户输入或不受信任的数据时，用 try/catch 包裹 stoi/stod；或者使用 C++17 的 std::from_chars——它不抛异常，通过返回值报告错误，性能也更好。

⚠️ 编程陷阱：string::find 返回 size_t（无符号），与 -1 比较出错

错误做法：
if (str.find("key") == -1) { ... }  // 编译警告：signed/unsigned 比较
现象：在某些平台上始终为 true 或始终为 false，取决于 -1 到 size_t 的隐式转换。

根本原因：find 返回 std::string::npos，它等于 size_t(-1)，即 size_t 的最大值（64 位系统上约 1.8e19）。虽然 -1 转换为 size_t 后恰好等于 npos，但这种写法触发编译器警告，且意图不清晰。

正确做法：始终与 std::string::npos 比较。

练习¶

实践题：编写函数 std::string zeroPad(int num, int width)，将数字转为指定宽度的零填充字符串（如 zeroPad(42, 6) 返回 "000042"）。用两种方式实现：to_string + insert，以及 ostringstream + setw + setfill。
分析题：在一个需要解析 100 万行 CSV 文件的场景中，估算使用 string::substr vs string_view::substr 的内存分配次数差异。假设每行有 10 个字段。

21.5 g2o 图文件格式 ⭐⭐⭐¶

动机：理解图优化的数据结构从文件格式开始¶

g2o（General Graph Optimization）是 SLAM 后端最常用的图优化库。它将 SLAM 问题建模为一个图：顶点（vertex）代表待优化的状态变量（机器人位姿、路标点），边（edge）代表观测约束（里程计、回环检测）。g2o 定义了一种文本格式（.g2o 文件）来存储和加载图结构。

理解 .g2o 文件格式有两个价值。第一，它让你在不看 g2o 源码的情况下就能理解图优化的输入数据结构——哪些是变量、哪些是约束、约束的置信度如何表示。第二，手写一个 .g2o 文件解析器是锻炼文件 I/O 和 stringstream 技能的绝佳练习。

.g2o 文本格式规范¶

一个 .g2o 文件由两类行组成：顶点行和边行。

2D 位姿图（VERTEX_SE2 / EDGE_SE2）

VERTEX_SE2 0 0.0 0.0 0.0
VERTEX_SE2 1 1.0 0.0 0.5
VERTEX_SE2 2 1.5 0.8 1.2
EDGE_SE2 0 1 1.03 0.02 0.49 500.0 0.0 0.0 500.0 0.0 500.0
EDGE_SE2 1 2 0.52 0.81 0.71 500.0 0.0 0.0 500.0 0.0 500.0
EDGE_SE2 2 0 -1.48 -0.78 -1.68 100.0 0.0 0.0 100.0 0.0 100.0

VERTEX_SE2 id x y theta：编号为 id 的 2D 位姿，位置 (x, y)，朝向角 theta。

EDGE_SE2 id1 id2 dx dy dtheta i11 i12 i13 i22 i23 i33：从顶点 id1 到顶点 id2 的相对观测（里程计或回环约束），测量值为 (dx, dy, dtheta)，后面 6 个数是信息矩阵（information matrix）的上三角元素。

信息矩阵是协方差矩阵的逆 \(\Omega = \Sigma^{-1}\)。它的含义是：这个观测有多"可信"。对角元素越大，对应分量的约束越强。例如上面的 500.0 0.0 0.0 500.0 0.0 500.0 表示 \(\Omega = 500 I_3\)，即三个自由度上的标准差都是 \(\sigma = 1/\sqrt{500} \approx 0.045\)。最后一条边（回环）的信息矩阵是 \(100 I_3\)，标准差约 0.1——回环检测的不确定性比里程计大，这在物理上是合理的。

3D 位姿图（VERTEX_SE3:QUAT / EDGE_SE3:QUAT）

VERTEX_SE3:QUAT 0 0.0 0.0 0.0 0.0 0.0 0.0 1.0
VERTEX_SE3:QUAT 1 1.0 0.0 0.5 0.0 0.0 0.2474 0.9689
EDGE_SE3:QUAT 0 1 1.0 0.0 0.5 0.0 0.0 0.2474 0.9689 500 0 0 0 0 0 500 0 0 0 0 500 0 0 0 500 0 0 500 0 500

VERTEX_SE3:QUAT id tx ty tz qx qy qz qw：3D 位姿，位置 + 四元数。注意四元数顺序为 qx qy qz qw。

EDGE_SE3:QUAT id1 id2 dx dy dz qx qy qz qw i11 i12 ... i66：3D 相对约束，后面 21 个数是 6x6 信息矩阵的上三角元素（\(6 \times 7 / 2 = 21\)）。

构建一个 g2o 文件解析器¶

下面构建一个完整的 .g2o 文件解析器，处理 2D 位姿图。这个练习综合运用了 ifstream、getline、stringstream 的所有技能。

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <iostream>
#include <stdexcept>
#include <cmath>

struct Vertex2D {
    int id;
    double x, y, theta;
};

struct Edge2D {
    int id_from, id_to;
    double dx, dy, dtheta;
    double info[6];  // 上三角：i11, i12, i13, i22, i23, i33
};

struct Graph2D {
    std::vector<Vertex2D> vertices;
    std::vector<Edge2D> edges;
};

Graph2D loadG2O(const std::string& filename) {
    std::ifstream ifs(filename);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open g2o file: " + filename);
    }

    Graph2D graph;
    std::string line;
    int line_num = 0;

    while (std::getline(ifs, line)) {
        ++line_num;
        if (line.empty() || line[0] == '#') continue;

        std::istringstream iss(line);
        std::string tag;
        iss >> tag;

        if (tag == "VERTEX_SE2") {
            Vertex2D v;
            if (!(iss >> v.id >> v.x >> v.y >> v.theta)) {
                throw std::runtime_error(
                    "Parse error at line " + std::to_string(line_num));
            }
            graph.vertices.push_back(v);
        } else if (tag == "EDGE_SE2") {
            Edge2D e;
            if (!(iss >> e.id_from >> e.id_to
                      >> e.dx >> e.dy >> e.dtheta)) {
                throw std::runtime_error(
                    "Parse error at line " + std::to_string(line_num));
            }
            for (int i = 0; i < 6; ++i) {
                if (!(iss >> e.info[i])) {
                    throw std::runtime_error(
                        "Info matrix parse error at line " +
                        std::to_string(line_num));
                }
            }
            graph.edges.push_back(e);
        } else if (tag == "FIX") {
            // FIX id: 固定某个顶点不参与优化
            int fixed_id;
            iss >> fixed_id;
        }
    }

    std::cout << "Loaded " << graph.vertices.size() << " vertices, "
              << graph.edges.size() << " edges\n";
    return graph;
}

为什么先读 tag 再根据 tag 分支？ 这是文本协议解析的标准模式——第一个字段是消息类型标识符，后续字段的格式由类型决定。HTTP 头、ROS 消息、g2o 文件都遵循这种模式。stringstream 的 >> 运算符天然适合这种"逐字段解析"的需求。

**g2o 自身的 load() 和 save() 方法**也使用类似的 iostream 机制读写 .g2o 文件。当你在 g2o 中调用 optimizer.load("input.g2o") 时，内部的解析逻辑与上面的代码结构几乎一致——先读 tag，根据 tag 查找已注册的 vertex/edge 工厂，调用对应的 read() 方法从流中读取数据。

保存 g2o 文件¶

保存操作是读取的逆过程——将图结构序列化为文本：

void saveG2O(const std::string& filename, const Graph2D& graph) {
    std::ofstream ofs(filename);
    if (!ofs.is_open()) {
        throw std::runtime_error("Cannot create g2o file: " + filename);
    }

    ofs << std::fixed << std::setprecision(6);

    for (const auto& v : graph.vertices) {
        ofs << "VERTEX_SE2 " << v.id << " "
            << v.x << " " << v.y << " " << v.theta << "\n";
    }

    for (const auto& e : graph.edges) {
        ofs << "EDGE_SE2 " << e.id_from << " " << e.id_to << " "
            << e.dx << " " << e.dy << " " << e.dtheta;
        for (int i = 0; i < 6; ++i) {
            ofs << " " << e.info[i];
        }
        ofs << "\n";
    }
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：信息矩阵的上三角元素数量算错

错误做法：2D 位姿有 3 个自由度，上三角元素有 \(3 \times 4 / 2 = 6\) 个，但新手可能按 \(3 \times 3 = 9\) 个来读。

现象：前几行解析正确，后面的行全部错位——因为多读了 3 个数，"吃"掉了下一行的数据。

根本原因：\(n \times n\) 对称矩阵的上三角（含对角线）有 \(n(n+1)/2\) 个独立元素。3D 位姿有 6 个自由度，上三角元素 \(6 \times 7 / 2 = 21\) 个，不是 36 个。

正确做法：上三角元素数 = \(n(n+1)/2\)。2D: 6 个，3D: 21 个。解析时严格按这个数量读取。

💡 概念误区：认为 EDGE 的两个 id 有方向性——id1 必须小于 id2

新手想法："EDGE_SE2 5 3 ... 是不是写错了？应该是 3 到 5 才对。"

实际上：g2o 的边是有向的——EDGE_SE2 id1 id2 dx dy dtheta 表示"从 id1 的参考系观测 id2 的相对位姿"。id1 > id2 通常表示回环约束（从当前帧回到之前访问过的帧）。交换方向需要对测量值取逆变换。

正确理解：边的方向决定了测量值的参考系。优化算法在计算误差时使用 \(e = Z^{-1} (T_{id1}^{-1} T_{id2})\)，其中 \(Z\) 是测量值——方向搞反会导致优化结果错误。

练习¶

实践题：从 g2o 的官方示例数据（sphere2500.g2o，可在 g2o GitHub 仓库的 data/ 目录找到）中，用你的解析器统计顶点数和边数。验证：应该有 2500 个顶点。
扩展题：扩展解析器以支持 VERTEX_SE3:QUAT 和 EDGE_SE3:QUAT。注意 3D 边的信息矩阵有 21 个上三角元素。
思考题：为什么 g2o 选择文本格式而不是二进制格式？考虑可读性、可调试性、跨平台兼容性和文件大小的权衡。对于大型图（10 万个顶点），文本格式会有什么问题？

21.6 二进制文件读写 ⭐⭐¶

动机：文本格式太慢了¶

KITTI 的一帧激光雷达点云包含约 12 万个点，每个点 4 个 float（x, y, z, intensity），数据量为 \(120000 \times 4 \times 4 = 1.92\) MB。如果用文本格式存储（每个 float 约 10 个字符），文件大小约 4.8 MB；用二进制格式只有 1.92 MB——文件大小减半。

但真正的差异不在文件大小，而在解析速度。文本格式需要逐字符解析（判断空格、小数点、正负号），将 ASCII 字符串转换为 IEEE 754 浮点数；二进制格式直接将原始字节 memcpy 到内存——没有任何解析步骤。对于 12 万个点的点云，文本解析需要约 50ms，二进制读取只需约 0.5ms——快 100 倍。在实时 SLAM 系统中，每帧省下的 50ms 可以用于更精细的优化。

reinterpret_cast 与二进制 I/O¶

C++ 的二进制文件读写通过 std::ifstream::read() 和 std::ofstream::write() 实现。它们的参数都是 char* 指针和字节数。为了将任意类型的数据指针转为 char*，需要使用 reinterpret_cast。

#include <fstream>
#include <vector>
#include <stdexcept>

struct Point4f {
    float x, y, z, intensity;
};

std::vector<Point4f> readKITTIBin(const std::string& filename) {
    std::ifstream ifs(filename, std::ios::binary | std::ios::ate);
    if (!ifs.is_open()) {
        throw std::runtime_error("Cannot open: " + filename);
    }

    std::streamsize file_size = ifs.tellg();
    ifs.seekg(0, std::ios::beg);

    if (file_size % sizeof(Point4f) != 0) {
        throw std::runtime_error("File size not aligned to Point4f");
    }

    size_t num_points = file_size / sizeof(Point4f);
    std::vector<Point4f> points(num_points);

    ifs.read(reinterpret_cast<char*>(points.data()),
             file_size);

    return points;
}

void writePointsBinary(const std::string& filename,
                       const std::vector<Point4f>& points) {
    std::ofstream ofs(filename, std::ios::binary);
    if (!ofs.is_open()) {
        throw std::runtime_error("Cannot create: " + filename);
    }

    ofs.write(reinterpret_cast<const char*>(points.data()),
              points.size() * sizeof(Point4f));
}

ios::ate 的含义：ate（at the end）让文件打开后指针定位到文件末尾，这样 tellg() 直接返回文件大小。然后用 seekg(0) 回到开头开始读取。这是获取文件大小的惯用手法。

reinterpret_cast 的含义：它告诉编译器"把这个指针当作另一种类型的指针来理解"。reinterpret_cast<char*>(points.data()) 把 Point4f* 解释为 char*——不改变底层数据，只改变指针的"解读方式"。这是二进制 I/O 的基本操作，也是 C++ 中少数需要使用 reinterpret_cast 的合法场景之一。

PCL 的二进制 PCD 文件¶

PCL（Point Cloud Library）定义了自己的 PCD（Point Cloud Data）文件格式，支持文本和二进制两种模式。在 SLAM 中，保存和加载点云地图通常使用二进制模式以获得最佳性能。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

void pclBinaryDemo() {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZI>);

    // 从二进制 PCD 文件加载
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("map.pcd", *cloud) == -1) {
        throw std::runtime_error("Cannot load PCD file");
    }

    // 保存为二进制 PCD 文件
    pcl::io::savePCDFileBinary("map_binary.pcd", *cloud);

    // 对比：保存为 ASCII PCD 文件（慢 10-100 倍）
    pcl::io::savePCDFileASCII("map_ascii.pcd", *cloud);
}

PCL 的 savePCDFileBinary 内部就是用 ostream::write 将点云数据的原始字节写入文件。文件头（PCD header）仍然是文本格式，包含点云的元信息（字段名、类型、宽高），这让工具可以在不读取全部数据的情况下了解文件内容。

文本 vs 二进制的全面对比¶

特性	文本格式	二进制格式
可读性	人眼可读	需要专用工具
文件大小	约 2-3 倍于二进制	最小
读写速度	慢（需要解析/格式化）	快（直接 memcpy）
跨平台	安全	需注意 endianness 和对齐
调试	容易（cat/grep）	困难（需要 hexdump）
精度	可能丢失（取决于格式）	无损（原始 IEEE 754）
典型应用	配置文件、小量数据	点云、深度图、大量数据

Endianness 问题¶

**Endianness（字节序）**是二进制 I/O 中必须考虑的问题。一个 32 位整数 0x01020304 在内存中的存储方式有两种：

Little-endian（小端序）：低位字节在前，04 03 02 01。x86/x86-64 架构使用。
Big-endian（大端序）：高位字节在前，01 02 03 04。网络字节序（TCP/IP）使用。

如果你在 x86 机器上写入一个二进制文件，然后在 ARM big-endian 机器上读取，数据会完全错乱。实际上，现代 ARM 处理器（如树莓派、NVIDIA Jetson）默认也是 little-endian，所以在 SLAM 的典型硬件上这个问题很少出现。但在设计跨平台的文件格式时，最好在文件头中记录字节序信息——注意 PCL 的 PCD 文件头中的 DATA 字段（ascii、binary、binary_compressed）标识的是**数据编码格式**而非字节序——PCD 格式本身不记录 endianness，二进制 PCD 文件使用写入平台的原生字节序。

#include <cstdint>

bool isLittleEndian() {
    uint32_t test = 0x01020304;
    uint8_t first_byte = *reinterpret_cast<uint8_t*>(&test);
    return first_byte == 0x04;  // little-endian: 低位在前
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：二进制读取时忘了 ios::binary 标志

错误做法：
std::ifstream ifs("data.bin");  // 缺少 ios::binary
ifs.read(reinterpret_cast<char*>(&data), sizeof(data));
现象：在 Windows 上，读取的数据与写入的不一致。在 Linux 上通常正常。

根本原因：不带 ios::binary 时，流以文本模式打开。Windows 的文本模式会将 \r\n（0x0D 0x0A）转换为 \n（0x0A），导致二进制数据中恰好出现 0x0D 0x0A 的字节被"吞掉"一个字节，后续数据全部错位。Linux 的文本模式和二进制模式没有区别（不做换行转换），所以 bug 只在 Windows 上出现。

正确做法：读写二进制数据时**始终加 std::ios::binary**。即使你只在 Linux 上开发，也要加——代码可能被移植到 Windows。

⚠️ 编程陷阱：对含有指针成员的结构体做二进制 I/O

错误做法：
struct BadStruct {
    std::string name;  // 内部有指针
    double value;
};
BadStruct s;
ofs.write(reinterpret_cast<const char*>(&s), sizeof(s));
现象：写入的是指针的地址值，不是字符串内容。读取后字符串指向无意义的内存地址。

根本原因：std::string 内部存储一个指向堆内存的指针。write 把指针的 8 字节地址写入文件，而不是字符串的实际内容。重新读取后，这个地址在新进程中无意义。

正确做法：只对 POD（Plain Old Data）类型的结构体做直接二进制 I/O。含有指针、虚函数表、标准库容器的类型必须手动序列化——先写长度再写内容。

内存映射文件（mmap）：零拷贝访问大文件¶

当点云文件达到几百 MB 甚至 GB 级别时，ifstream::read 把整个文件读入 vector 的做法有两个问题：第一，需要分配和文件同样大的内存——如果同时加载多个大文件，内存可能不够。第二，从磁盘到用户态缓冲区要经过两次拷贝（磁盘→内核页缓存→用户空间），对于只需要读取的场景，这次从内核到用户空间的拷贝是浪费。

内存映射文件（memory-mapped file，POSIX 上用 mmap，Windows 上用 CreateFileMapping）把文件直接映射到进程的虚拟地址空间。程序通过指针直接访问文件内容，操作系统按需把需要的页从磁盘加载到内存——不读到的页不占物理内存，读到的页由操作系统自动管理缓存。

类比：ifstream::read 像是把一本书整本复印到你的桌子上再阅读。mmap 像是把书放在书架上，你直接翻到要看的那一页——不看的页不占桌面空间。

#include <sys/mman.h>  // POSIX mmap
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <cstddef>
#include <stdexcept>
#include <string>

class MappedFile {
public:
    explicit MappedFile(const std::string& path) {
        fd_ = open(path.c_str(), O_RDONLY);
        if (fd_ < 0) throw std::runtime_error("Cannot open: " + path);

        struct stat st;
        fstat(fd_, &st);
        size_ = static_cast<size_t>(st.st_size);

        // MAP_PRIVATE: 写时拷贝，不影响原文件
        // PROT_READ: 只读映射
        data_ = static_cast<const char*>(
            mmap(nullptr, size_, PROT_READ, MAP_PRIVATE, fd_, 0));
        if (data_ == MAP_FAILED) {
            close(fd_);
            throw std::runtime_error("mmap failed: " + path);
        }
    }

    ~MappedFile() {
        if (data_ != MAP_FAILED) munmap(const_cast<char*>(data_), size_);
        if (fd_ >= 0) close(fd_);
    }

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

    const char* data() const { return data_; }
    size_t size() const { return size_; }

private:
    int fd_ = -1;
    const char* data_ = nullptr;
    size_t size_ = 0;
};

在点云处理中的应用：

#include <cstring>
#include <vector>

struct PointXYZI {
    float x, y, z, intensity;
};

std::vector<PointXYZI> loadKITTIBinMmap(const std::string& path) {
    MappedFile file(path);

    // KITTI .bin 文件：连续的 float x, y, z, intensity
    size_t point_count = file.size() / sizeof(PointXYZI);
    std::vector<PointXYZI> points(point_count);

    // 直接从映射内存拷贝，操作系统按需加载页
    std::memcpy(points.data(), file.data(), file.size());

    return points;
}

反事实推理：如果不用 mmap 会怎样？对于 100MB 的点云文件，ifstream::read 需要先 malloc 100MB 缓冲区，再等待 100MB 数据从磁盘完全读入后才能开始处理。mmap 则可以在映射完成后立即通过指针访问任意位置——操作系统只在你实际访问某一页时才从磁盘加载那一页（缺页中断），对于只需要读取文件前 10% 的场景（如只读文件头判断格式），性能差距巨大。

mmap 的局限：mmap 不适合所有场景。对于小文件（<1MB），read 的简单性更有价值。对于需要频繁追加写入的文件（如日志），mmap 的固定大小映射不方便扩展。在 32 位系统上，虚拟地址空间有限（4GB），不能映射超大文件。

练习¶

实践题：编写程序生成 100 万个随机 3D 点（float x, y, z），分别以文本格式和二进制格式保存。比较：(a) 文件大小差异，(b) 写入耗时差异，(c) 读取耗时差异。
设计题：设计一个简单的二进制点云文件格式，包含：文件头（魔数 4 字节 + 点数 4 字节 + 字节序标志 1 字节）、数据体（连续的 float x, y, z 三元组）。实现读写函数，并验证在写入后读取数据完全一致。
对比题：分别用 ifstream::read 和 mmap 读取一个 50MB 的 KITTI .bin 文件。测量两者的加载耗时差异。解释在什么条件下 mmap 的优势更明显（提示：只读取文件前 N 个点 vs 读取全部）。

21.7 fmt 库与 std::format ⭐⭐⭐¶

动机：iostream 格式化的痛苦¶

C++ 的 iostream 格式化输出语法非常繁琐。要输出一个带前导零的 6 位帧号和一个 6 位小数的时间戳，需要：

#include <iostream>
#include <iomanip>

std::cout << "Frame " << std::setw(6) << std::setfill('0') << frame_id
          << " at t=" << std::fixed << std::setprecision(6) << timestamp
          << "s" << std::endl;

这段代码有三个问题。第一，setw 只对紧接着的下一次输出生效（是"一次性"的），而 setprecision 和 fixed 则是"粘性"的（直到被更改）——这种不一致性是 bug 的温床。第二，格式化意图被操纵符打散，难以一眼看出输出的最终效果。第三，iostream 的格式化性能较差——由于 locale 支持和虚函数调用，比 printf 慢 2-5 倍。

fmt 库：现代 C++ 的格式化标准¶

fmt（原名 cppformat，作者 Victor Zverovich）是一个高性能的格式化库，已被 C++20 标准采纳为 std::format。它使用 Python 风格的花括号占位符语法，在编译期检查格式字符串的类型安全性，性能比 iostream 快 5-10 倍。

SLAM 生态中，spdlog——最常用的高性能日志库——内部使用 fmt 作为格式化引擎。当你写 spdlog::info("Frame {} at {:.6f}s", id, time) 时，格式化工作由 fmt 完成。

#include <fmt/core.h>
#include <fmt/format.h>
#include <string>

void fmtBasicDemo() {
    int frame_id = 42;
    double timestamp = 1305031102.175304;

    // 基本用法：{} 自动推断类型
    std::string s1 = fmt::format("Frame {} at t={}s", frame_id, timestamp);

    // 格式规范：{:规范}
    // {:06d} = 6 位宽，前导零，十进制整数
    // {:.6f} = 6 位小数，定点表示
    std::string s2 = fmt::format("Frame {:06d} at t={:.6f}s",
                                  frame_id, timestamp);

    // 直接输出到 stdout
    fmt::print("Processing: {}\n", s2);

    // 输出到文件
    FILE* fp = fopen("log.txt", "w");
    if (fp) {
        fmt::print(fp, "Frame {:06d} at t={:.6f}s\n",
                   frame_id, timestamp);
        fclose(fp);
    }
}

fmt 格式规范详解¶

fmt 的格式规范语法为 {[index]:[fill][align][width][.precision][type]}：

元素	含义	示例
`index`	参数索引（可省略）	`{0}` = 第一个参数
`fill`	填充字符（默认空格）	`{:0>6}` 用 0 填充
`align`	对齐方式：`<` 左 `>` 右 `^` 居中	`{:<10}` 左对齐
`width`	最小宽度	`{:10}` 至少 10 字符宽
`.precision`	小数位数或字符串截断	`{:.6f}` 6 位小数
`type`	类型标识：`d`/`f`/`e`/`s`/`x`	`{:x}` 十六进制

#include <fmt/core.h>

void fmtFormatSpec() {
    // 整数格式
    fmt::print("{:d}\n", 42);        // "42"
    fmt::print("{:06d}\n", 42);      // "000042"
    fmt::print("{:#x}\n", 255);      // "0xff"
    fmt::print("{:b}\n", 42);        // "101010" (二进制)

    // 浮点格式
    fmt::print("{:f}\n", 3.14);       // "3.140000"
    fmt::print("{:.2f}\n", 3.14);     // "3.14"
    fmt::print("{:e}\n", 0.001);      // "1.000000e-03"
    fmt::print("{:.9f}\n", 1305031102.175304);  // "1305031102.175304000"

    // 字符串对齐
    fmt::print("{:<15} {:>10.3f}\n", "Position:", 3.14);
    // "Position:            3.140"

    // 命名参数（fmt 10+）
    fmt::print("({x:.2f}, {y:.2f})\n",
               fmt::arg("x", 1.5), fmt::arg("y", 2.3));
}

fmt vs iostream vs printf 性能对比¶

特性	printf	iostream	fmt::format	std::format (C++20)
类型安全	否	是	是	是
编译期检查	否	部分	是（fmt 8+）	是
性能	快	慢（locale + 虚函数）	最快	接近 fmt
可扩展性	否	是（operator<<）	是（formatter 特化）	是
可读性	中等	差（操纵符散乱）	好（Python 风格）	好

为什么 fmt 比 iostream 快 5-10 倍？ 三个原因。第一，fmt 不使用 locale——SLAM 中不需要千分位分隔符或本地化数字格式。第二，fmt 没有虚函数调用——iostream 的 operator<< 通过虚函数分发到 streambuf，每次输出都有虚函数开销。第三，fmt 在编译期分析格式字符串，生成专用的格式化代码——类似于 printf 的手写特化版本。

C++20 std::format 与 fmt 的关系¶

std::format（<format> 头文件）是 C++20 标准的一部分，基于 fmt 库设计。它的语法与 fmt 几乎完全一致：

// C++20 std::format（需要 g++ -std=c++20 或更高）
#include <format>
#include <string>

void stdFormatDemo() {
    // 基本格式化——语法与 fmt::format 完全一致
    std::string s = std::format("Frame {:06d} at t={:.6f}s", 42, 1.234);

    // 自定义类型的 std::formatter 特化（类似 fmt::formatter）
    // 需要特化 std::formatter<MyType>，实现 parse() 和 format()
}

截至目前，GCC 13+、Clang 17+、MSVC 2022 已完整支持 std::format。但在实际 SLAM 项目中，大多数代码库仍然使用 fmt 库——因为它支持 C++11/14/17，不要求 C++20 编译器，而且 spdlog 已经把 fmt 作为依赖引入了。

C++23 std::print：格式化输出的终极形式¶

C++23 引入了 std::print 和 std::println，直接将格式化输出写到 stdout 或指定的文件流，不再需要先构造 std::string 再输出。这是 fmt::print 进入标准库的最终形态。

// C++23 std::print（需要 g++ -std=c++23 或更高）
// GCC 14+、Clang 18+ 支持
#include <print>

void stdPrintDemo() {
    int frame_id = 42;
    double ape = 0.0342;

    // 直接输出到 stdout，自动换行
    std::println("Frame {:06d}: APE = {:.4f} m", frame_id, ape);

    // 不加换行
    std::print("Processing frame {}...", frame_id);

    // 输出到 stderr
    std::println(stderr, "Warning: feature count low");
}

std::print 相比 std::cout 的三大优势：第一，格式字符串集中表达输出意图，可读性远好于散乱的 << 链；第二，编译期类型检查，不可能出现 printf 的格式符和类型不匹配问题；第三，性能接近 printf，远好于 iostream。

从 fmt 到标准库的迁移路径：

版本	fmt 接口	标准库等价物	编译器要求
C++11-17	`fmt::format(...)`	无	引入 fmt 库
C++20	`fmt::format(...)`	`std::format(...)`	GCC 13+ / Clang 17+
C++23	`fmt::print(...)`	`std::print(...)` / `std::println(...)`	GCC 14+ / Clang 18+

迁移建议：如果你的项目已经通过 spdlog 引入了 fmt，继续使用 fmt 即可——API 完全一致，无迁移成本。如果你开始一个新项目并且可以要求 C++20+，使用 std::format 可以减少一个外部依赖。C++23 的 std::print 是未来方向，但短期内 fmt 的生态更成熟。

SLAM 中的实际应用¶

#include <fmt/core.h>
#include <spdlog/spdlog.h>

void slamLogging() {
    int frame = 100;
    double ape = 0.0342;
    int features = 1523;

    // spdlog 使用 fmt 语法
    spdlog::info("Frame {:06d}: APE={:.4f}m, features={}", frame, ape, features);

    // 生成带序号的文件名
    std::string cloud_file = fmt::format("cloud_{:06d}.pcd", frame);
    std::string depth_file = fmt::format("depth_{:06d}.png", frame);

    // 格式化轨迹输出
    double tx = 1.5, ty = 2.3, tz = -0.4;
    std::string tum_line = fmt::format("{:.9f} {:.6f} {:.6f} {:.6f} 0 0 0 1",
                                        1305031102.175, tx, ty, tz);
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：格式字符串中的花括号数量与参数数量不匹配

错误做法：
fmt::format("x={}, y={}", 1.0);  // 2 个占位符，1 个参数
现象：编译期报错（fmt 8+）或运行时抛出 fmt::format_error 异常。不会像 printf 那样读取栈上的垃圾值。

根本原因：fmt 在编译期检查格式字符串，发现占位符数量和参数数量不匹配时直接拒绝编译。这是 fmt 相比 printf 的安全优势。

正确做法：确保 {} 的数量与参数数量一致。IDE 的 fmt 插件可以实时高亮不匹配的情况。

💡 概念误区：认为 fmt 和 iostream 不能混用

新手想法："项目里已经用了 cout，不能再引入 fmt 了吧？"

实际上：fmt::print 默认输出到 stdout（与 printf 共享），和 iostream 可以共存。但要注意两者的缓冲区可能不同步。如果需要严格的输出顺序，可以在混用前调用 std::ios::sync_with_stdio(true)（默认就是 true）。更好的做法是全面迁移到 fmt 或 spdlog，不再直接使用 iostream 输出。

练习¶

实践题：用 fmt 实现一个 formatTable 函数，接受列标题和数据矩阵，输出对齐的表格。例如输入 {"Name", "APE", "RPE"} 和对应数据，输出：
```
Name           APE       RPE
ORB-SLAM3     0.0342    0.0215
LIO-SAM       0.0156    0.0098
```
性能题：编写基准测试，比较 fmt::format、std::ostringstream、sprintf 格式化 "Frame {:06d} at {:.6f}s" 100 万次的耗时。

21.8 std::filesystem (C++17) ⭐⭐¶

动机：跨平台的路径操作¶

SLAM 数据集的目录结构通常是固定的——KITTI 的结构为 sequences/00/velodyne/000001.bin，你需要拼接路径、遍历目录下的所有 .bin 文件、检查输出目录是否存在并创建。在 C++17 之前，路径操作需要使用 POSIX API（opendir/readdir/stat）或 Boost.Filesystem——前者不跨平台，后者需要额外依赖。C++17 将 Boost.Filesystem 标准化为 <filesystem>，提供了跨平台、类型安全的路径和文件系统操作。

std::filesystem::path——路径是一等公民¶

std::filesystem::path 不是 std::string——它是一个专门表示文件系统路径的类型，理解目录分隔符、文件名、扩展名等路径概念。

#include <filesystem>
#include <iostream>

namespace fs = std::filesystem;

void pathDemo() {
    // 从字符串构造路径
    fs::path base = "/data/kitti/sequences/00";

    // 用 / 运算符拼接路径（跨平台，自动处理分隔符）
    fs::path velodyne_dir = base / "velodyne";
    fs::path bin_file = velodyne_dir / "000001.bin";
    // 结果："/data/kitti/sequences/00/velodyne/000001.bin"

    // 提取路径组成部分
    std::cout << "Parent:    " << bin_file.parent_path() << "\n";    // .../velodyne
    std::cout << "Filename:  " << bin_file.filename() << "\n";       // 000001.bin
    std::cout << "Stem:      " << bin_file.stem() << "\n";           // 000001
    std::cout << "Extension: " << bin_file.extension() << "\n";      // .bin

    // 替换扩展名
    fs::path label_file = bin_file;
    label_file.replace_extension(".label");  // 000001.label
}

为什么用 path 而不是 string 拼接路径？ 第一，path 的 / 运算符会自动处理分隔符——"dir" / "file" 不会变成 "dirfile"。第二，path 理解操作系统差异——在 Windows 上会使用 \，在 Linux 上使用 /。第三，path 的成员函数（parent_path、stem、extension）语义明确，比手动用 rfind('/') 和 substr 切分更安全。

文件系统查询与操作¶

#include <filesystem>
#include <iostream>

namespace fs = std::filesystem;

void fsOperationsDemo() {
    fs::path output_dir = "results/slam_output";

    // 检查路径是否存在
    if (!fs::exists(output_dir)) {
        // 递归创建目录（包括中间目录）
        fs::create_directories(output_dir);
        std::cout << "Created: " << output_dir << "\n";
    }

    // 检查是文件还是目录
    if (fs::is_directory(output_dir)) {
        std::cout << output_dir << " is a directory\n";
    }

    // 获取文件大小
    fs::path cloud = "/data/cloud.pcd";
    if (fs::exists(cloud)) {
        auto size = fs::file_size(cloud);
        std::cout << "File size: " << size << " bytes\n";
    }

    // 复制和重命名
    fs::copy_file("src.txt", "dst.txt",
                  fs::copy_options::overwrite_existing);
    fs::rename("old_name.txt", "new_name.txt");
}

目录遍历：扫描数据集文件¶

SLAM 数据集加载的典型需求是：扫描某个目录下的所有 .bin 或 .pcd 文件，按文件名排序后依次处理。

#include <filesystem>
#include <vector>
#include <string>
#include <algorithm>

namespace fs = std::filesystem;

std::vector<fs::path> scanPointCloudFiles(const fs::path& dir,
                                           const std::string& extension) {
    if (!fs::exists(dir) || !fs::is_directory(dir)) {
        throw std::runtime_error("Invalid directory: " + dir.string());
    }

    std::vector<fs::path> files;

    for (const auto& entry : fs::directory_iterator(dir)) {
        if (entry.is_regular_file() &&
            entry.path().extension() == extension) {
            files.push_back(entry.path());
        }
    }

    // 按文件名排序（确保帧的时间顺序）
    std::sort(files.begin(), files.end());

    return files;
}

void processDataset() {
    fs::path kitti_dir = "/data/kitti/sequences/00/velodyne";
    auto bin_files = scanPointCloudFiles(kitti_dir, ".bin");

    for (const auto& f : bin_files) {
        // readKITTIBin(f.string()) ...
        // f.stem() 给出 "000001" 等帧号
    }
}

directory_iterator vs recursive_directory_iterator。前者只遍历一层目录，后者递归遍历所有子目录。扫描数据集通常用前者（因为所有文件在同一个目录下）。搜索项目中所有 .cpp 文件时用后者。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：fs::path 到 std::string 的转换在不同平台上行为不同

错误做法：直接将 fs::path 传给接受 std::string 的函数。

现象：在 Windows 上，path::string() 返回 Windows 本地编码的字符串，而 path::u8string() 返回 UTF-8 编码。如果路径包含中文或其他非 ASCII 字符，两者不同。

根本原因：fs::path 在 Windows 上内部使用宽字符（wchar_t），在 Linux 上使用窄字符（char）。string() 方法做了平台相关的转换。

正确做法：如果需要 UTF-8 编码的路径字符串，使用 path::u8string()。在纯 Linux 环境下（SLAM 开发的主要平台），string() 和 u8string() 通常返回相同结果。

💡 概念误区：认为 directory_iterator 返回的文件顺序是确定的

新手想法："directory_iterator 会按文件名字母顺序返回文件。"

实际上：C++ 标准**不保证** directory_iterator 的遍历顺序。不同操作系统、不同文件系统（ext4、NTFS、ZFS）可能返回不同的顺序。在 Linux ext4 上通常按 inode 顺序返回，与文件名顺序无关。

正确做法：收集所有文件路径到 vector 后显式 std::sort，如上面的代码所示。

练习¶

实践题：编写函数 prepareOutputDir(const fs::path& dir)，如果目录不存在则创建；如果存在且非空则打印警告并要求用户确认（通过 std::cin）。在 SLAM 实验中，避免意外覆盖之前的结果很重要。
实践题：编写一个程序，扫描给定目录下所有 .pcd 文件，统计总大小（file_size），并按大小排序输出前 10 个最大的文件及其大小（用人类可读的格式，如 12.3 MB）。

21.9 序列化框架概览 ⭐⭐⭐¶

动机：SLAM 地图需要持久化¶

一个 SLAM 系统运行几个小时建好的地图，需要保存到磁盘以便下次启动时直接加载——而不是从头重建。这个"将内存中的对象层次结构转换为字节流（序列化），再从字节流恢复为对象（反序列化）"的过程，就是序列化（serialization）。

SLAM 地图的序列化并非简单的"把一个结构体 write 到文件"——它涉及复杂的对象图（object graph）。以 ORB-SLAM3 为例，一个 Atlas 包含多个 Map，每个 Map 包含数千个 KeyFrame 和 MapPoint，KeyFrame 之间通过 covisibility graph 互相引用，MapPoint 又引用观测到它的 KeyFrame。这种复杂的指针关系不能用简单的二进制 I/O 保存——需要序列化框架来处理对象引用、版本兼容、跨平台字节序等问题。

Boost.Serialization——ORB-SLAM3 的选择¶

ORB-SLAM3 使用 Boost.Serialization 实现 Atlas 的持久化存储。这个框架的核心机制是侵入式序列化（intrusive serialization）——每个需要序列化的类内部定义一个 serialize 模板方法：

#include <boost/serialization/serialization.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <fstream>
#include <vector>

struct MapPoint {
    int id;
    float x, y, z;
    std::vector<int> observer_keyframe_ids;

    template <class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & id;
        ar & x & y & z;
        ar & observer_keyframe_ids;
    }
};

struct SimpleMap {
    std::vector<MapPoint> points;
    std::string map_name;

    template <class Archive>
    void serialize(Archive& ar, const unsigned int version) {
        ar & map_name;
        ar & points;
    }
};

void saveMap(const std::string& filename, const SimpleMap& map) {
    std::ofstream ofs(filename, std::ios::binary);
    boost::archive::binary_oarchive oa(ofs);
    oa << map;
}

SimpleMap loadMap(const std::string& filename) {
    std::ifstream ifs(filename, std::ios::binary);
    boost::archive::binary_iarchive ia(ifs);
    SimpleMap map;
    ia >> map;
    return map;
}

ar & 运算符的双向含义。当 Archive 是输出 archive 时，ar & x 等价于 ar << x（序列化）；当 Archive 是输入 archive 时，ar & x 等价于 ar >> x（反序列化）。这让你用一个 serialize 函数同时处理两个方向，避免代码重复。

ORB-SLAM3 中的实际使用。ORB-SLAM3 的 KeyFrame.h、MapPoint.h、Map.h、Atlas.h 中都定义了 serialize 方法。Atlas::PreSave() 和 Atlas::PostLoad() 在序列化前后处理指针到 ID 的转换。编译 ORB-SLAM3 需要 libboost-serialization-dev 包——如果缺少这个依赖，CMake 配置阶段就会报错。

Boost.Serialization 的优势和劣势：

优势	劣势
与 C++ 深度集成，支持继承、多态	侵入式——需要修改类定义
自动处理 STL 容器和智能指针	编译慢（大量模板实例化）
支持版本号（`version`参数）	生成的文件不跨语言
支持文本/二进制/XML 三种格式	库体积大（libboost-serialization 约 2MB）

Protobuf——Cartographer 的选择¶

Google 的 Protocol Buffers（Protobuf）使用独立的 .proto 文件定义数据结构，通过 protoc 编译器生成 C++（或 Python、Java 等）的序列化/反序列化代码。Cartographer 使用 Protobuf 序列化地图和轨迹数据。

Protobuf 的核心特点是**语言无关、平台无关**。一个 .proto 文件可以同时生成 C++ 和 Python 的代码——这让机器人系统中不同语言编写的组件可以共享数据格式。

// map_point.proto
syntax = "proto3";

message MapPoint {
  int32 id = 1;
  float x = 2;
  float y = 3;
  float z = 4;
  repeated int32 observer_ids = 5;
}

message PointCloudMap {
  string name = 1;
  repeated MapPoint points = 2;
}

// 使用 protoc 生成的 C++ 代码（map_point.pb.h / map_point.pb.cc）
#include "map_point.pb.h"
#include <fstream>

void saveMapProto(const std::string& filename) {
    PointCloudMap map;
    map.set_name("slam_map");

    auto* pt = map.add_points();
    pt->set_id(0);
    pt->set_x(1.0f);
    pt->set_y(2.0f);
    pt->set_z(3.0f);

    std::ofstream ofs(filename, std::ios::binary);
    map.SerializeToOstream(&ofs);
}

Protobuf vs JSON 的性能差距。Protobuf 使用紧凑的二进制编码（varint、zigzag 等），序列化后的数据量比 JSON 小 3-10 倍，解析速度快 20-100 倍。对于包含数百万 MapPoint 的大型地图，这个差距是"秒级加载"和"分钟级加载"的区别。

nlohmann/json——配置与轻量数据交换¶

对于配置文件和小规模数据交换，JSON 的可读性和通用性比性能更重要。nlohmann/json 是 C++ 社区最流行的 JSON 库——header-only，API 设计直觉化，性能满足大部分非性能关键场景。

#include <nlohmann/json.hpp>
#include <fstream>

using json = nlohmann::json;

void jsonDemo() {
    // 创建 JSON 对象
    json config;
    config["camera"]["fx"] = 718.856;
    config["camera"]["fy"] = 718.856;
    config["camera"]["cx"] = 607.1928;
    config["camera"]["cy"] = 185.2157;
    config["max_features"] = 2000;
    config["use_gpu"] = false;

    // 保存到文件（缩进 4 空格）
    std::ofstream ofs("config.json");
    ofs << config.dump(4);

    // 从文件加载
    std::ifstream ifs("config.json");
    json loaded = json::parse(ifs);

    double fx = loaded["camera"]["fx"].get<double>();
}

cereal——现代 header-only 替代¶

cereal 是 Boost.Serialization 的现代替代品——header-only（无需链接库），API 更简洁，编译更快。它支持二进制、JSON、XML 三种输出格式。

#include <cereal/archives/binary.hpp>
#include <cereal/types/vector.hpp>
#include <cereal/types/string.hpp>
#include <fstream>
#include <vector>

struct Landmark {
    int id;
    float x, y, z;

    template <class Archive>
    void serialize(Archive& ar) {
        ar(id, x, y, z);  // 比 Boost 更简洁
    }
};

void cerealDemo() {
    std::vector<Landmark> landmarks = {{0, 1.0f, 2.0f, 3.0f},
                                        {1, 4.0f, 5.0f, 6.0f}};

    // 序列化
    {
        std::ofstream ofs("landmarks.bin", std::ios::binary);
        cereal::BinaryOutputArchive oar(ofs);
        oar(landmarks);
    }

    // 反序列化
    std::vector<Landmark> loaded;
    {
        std::ifstream ifs("landmarks.bin", std::ios::binary);
        cereal::BinaryInputArchive iar(ifs);
        iar(loaded);
    }
}

四种框架的全面对比¶

特性	Boost.Serialization	Protobuf	nlohmann/json	cereal
侵入式	是	否（.proto 定义）	否	是
Header-only	否	否	是	是
跨语言	否	是（10+语言）	是（JSON 通用）	否
二进制格式	是	是	否	是
文件大小	中等	最小	最大	中等
编解码速度	中等	快	慢	快
SLAM 中的使用	ORB-SLAM3 Atlas	Cartographer 地图	配置文件	较少
学习曲线	陡峭	中等（需要 protoc）	平缓	平缓
版本兼容	是（version 参数）	是（字段编号）	是（按键名查找）	有限

选择建议：如果你是新项目——配置文件用 nlohmann/json，大规模数据持久化用 cereal 或 Protobuf。如果你需要与 ORB-SLAM3 代码交互——必须学 Boost.Serialization。如果你的系统有多语言组件（C++ 后端 + Python 前端）——Protobuf 是最佳选择。

本质洞察：序列化框架的核心取舍在于"可移植性 vs 性能 vs 开发效率"三角。JSON 最可移植（任何语言都能解析）但最慢、最大。原始二进制最快最小但完全不可移植（换个编译器可能就读不了）。Protobuf 和 cereal 在中间地带——用 schema 换取跨版本/跨语言的兼容性，同时保持接近原始二进制的性能。选择框架时，先问"谁需要读这个数据"——只有自己的 C++ 代码读，用 cereal；Python 脚本也要读，用 Protobuf；人要直接看，用 JSON。

类比：序列化框架之间的区别，类似于不同的自然语言。JSON 是英语——全世界都能理解，但表达复杂概念时冗长。Protobuf 是世界语——设计出来就是为了跨文化交流，紧凑高效。Boost.Serialization 是方言——和母语使用者沟通效率最高，但外人听不懂。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Boost.Serialization 版本不匹配导致加载失败

错误做法：修改了类的 serialize 函数（添加/删除字段）后，尝试加载旧版本保存的文件。

现象：程序崩溃或读取出垃圾数据。

根本原因：Boost.Serialization 的二进制格式不包含字段元信息——它假设读写双方的 serialize 函数完全一致。如果你在 serialize 中新增了一个字段，旧文件中没有这个字段的数据，读取时会越界。

正确做法：使用 version 参数实现版本兼容：
template <class Archive>
void serialize(Archive& ar, const unsigned int version) {
    ar & id & x & y & z;
    if (version >= 1) {
        ar & confidence;  // 新版本才有的字段
    }
}
BOOST_CLASS_VERSION(MapPoint, 1)
🧠 思维陷阱：对所有数据都使用 JSON 序列化

新手想法："JSON 可读性好，用它保存所有数据——包括点云地图。"

实际上：一个包含 100 万个点的点云，JSON 格式约 100MB（每个点约 100 字节的文本），解析时间约 10 秒。二进制格式只有 12MB（每个点 12 字节），加载时间 < 0.1 秒。JSON 适合配置文件（几 KB 到几 MB），不适合大规模数值数据。

正确做法：按数据规模选择格式。配置/参数/少量结构化数据 -> JSON。大规模数值数据（点云、地图、轨迹） -> 二进制（Protobuf/cereal/原始二进制）。需要跨语言 + 大量数据 -> Protobuf。

练习¶

实践题：用 nlohmann/json 实现一个 SLAM 配置加载器：从 JSON 文件读取相机内参（fx, fy, cx, cy）、最大特征点数、是否启用回环检测等参数。加入类型检查——如果 fx 的值不是数字，抛出有意义的错误信息。
对比题：创建一个包含 1000 个 3D 点（float x, y, z）的数据集。分别用 nlohmann/json、cereal 二进制、原始 write 二进制三种方式保存。比较文件大小和读写耗时。
思考题：ORB-SLAM3 为什么选择 Boost.Serialization 而不是 Protobuf？考虑以下因素：ORB-SLAM3 是纯 C++ 项目，需要序列化包含复杂继承层次和指针关系的对象图，Protobuf 不支持指针序列化。

21.10 工程边界与验证清单 ⭐⭐¶

这一节解决什么问题：文件 I/O 的代码通常看起来很简单，但它处在 SLAM 系统的入口和出口。一旦解析边界处理不严，后面的 Eigen、位姿图和评测工具都会接收错误数据。本节把"能读出来"升级为"读错时能定位、写出后能验证、跨平台仍一致"。

动机：I/O 不是外围功能，而是数据契约¶

在 Mini-LIO 中，一条轨迹从磁盘读入后会经历：文本解析 → 位姿构造 → 李群优化 → 轨迹评测。文件 I/O 就像传感器驱动的离线版本：它把外部世界的数据转换为程序内部对象。如果这个转换少读一列、四元数顺序错一位、时间戳被截断，后端优化看到的就是一个自洽但错误的世界。

如果不做边界验证会怎样？最典型的现象不是程序马上崩溃，而是轨迹评测差一点、优化多跑几次才发散、回环约束偶尔异常。这类问题最难查，因为错误发生在通用库·文件IO，症状却出现在通用库·Ceres-通用库·OpenCV。

本质洞察：I/O 模块的职责不是"尽量把文件读进来"，而是**在外部协议和内部类型之间建立可验证的契约**。能恢复的格式差异要明确兼容，不能恢复的歧义要尽早报错。

文本解析的四层边界¶

边界	需要验证什么	失败后的处理	典型工具
文件级	文件是否存在、是否为空、是否可读	抛出带路径的异常	`ifstream::is_open()`
行级	空行、注释行、列数是否正确	跳过或报出行号	`getline` + 行号
字段级	类型转换是否成功、是否有多余字段	报出字段名和原始行	`stringstream`
语义级	时间戳单调、四元数归一、矩阵接近 SO(3)	拒绝或归一化并记录	Eigen 范数检查

这四层边界对应不同的错误定位粒度。只检查 ifs.good() 相当于只检查文件级边界，远远不够。一个完整的 KITTI/TUM 解析器应该能告诉你："第 137 行四元数范数为 0.82"，而不是只说"轨迹文件格式错误"。

解析器验证模板¶

下面的代码展示一个可复用的行解析模式。关键点不是代码多复杂，而是每个错误都带有**文件名、行号、原始文本和失败原因**。这些信息会直接决定调试时间是 5 分钟还是 5 小时。

#include <cmath>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

struct TumPoseLine {
    double timestamp = 0.0;
    double tx = 0.0, ty = 0.0, tz = 0.0;
    double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
};

TumPoseLine parseTumPoseLine(const std::string& filename,
                             int line_no,
                             const std::string& line) {
    std::istringstream iss(line);
    TumPoseLine pose;

    // operator>> 会自动跳过多个空格和制表符；失败时 failbit 被置位。
    if (!(iss >> pose.timestamp >> pose.tx >> pose.ty >> pose.tz
              >> pose.qx >> pose.qy >> pose.qz >> pose.qw)) {
        throw std::runtime_error(filename + ":" + std::to_string(line_no) +
                                 ": TUM pose expects 8 numeric fields: " + line);
    }

    std::string extra;
    if (iss >> extra) {
        throw std::runtime_error(filename + ":" + std::to_string(line_no) +
                                 ": extra field after qw: " + extra);
    }

    const double q_norm = std::sqrt(pose.qx * pose.qx + pose.qy * pose.qy +
                                    pose.qz * pose.qz + pose.qw * pose.qw);
    if (std::abs(q_norm - 1.0) > 1e-3) {
        throw std::runtime_error(filename + ":" + std::to_string(line_no) +
                                 ": quaternion norm is " + std::to_string(q_norm));
    }

    return pose;
}

对象生命周期边界也要说清楚：上面函数返回的是值类型 TumPoseLine，不返回指向 line.c_str() 的指针，也不返回 std::string_view。这是故意的，因为 line 在下一次循环会被覆盖；把视图存到容器里会形成悬垂引用。

输出文件的可回放原则¶

SLAM 输出文件不是只给人看的日志，而是下一轮评测、回放和优化的输入。因此输出函数应满足三条规则：

确定性：相同输入多次运行，输出行顺序、浮点精度、字段数量一致。
可逆性：write(read(file)) 之后再次读取，关键数值误差在设定阈值内。
可诊断性：写文件失败时能区分权限、路径不存在、磁盘空间不足等问题。

数据类型	推荐输出策略	验证方式
轨迹文本	固定精度 `std::setprecision(9)` 或 `fmt`	读写往返比较位姿误差
点云二进制	明确字段顺序和字节数	文件大小 = 点数 × 单点字节数 + 头部
配置 JSON	保留字段名和默认值	schema/类型检查
图优化文件	固定顶点/边排序	重新读取后顶点数、边数一致

二进制文件尤其需要版本字段。没有版本号时，你只能根据文件大小猜测字段顺序；一旦 PointXYZI 扩展为 PointXYZIRT，旧文件和新文件会在静默错位中互相污染。

线程与异常边界¶

文件读写本身通常不是实时线程的工作。在线 SLAM 中，前端跟踪线程应避免直接写大文件，否则磁盘抖动会造成帧间时间不稳定。推荐的边界是：

场景	推荐做法	不推荐做法
实时跟踪	将轨迹点写入无锁队列，由后台线程落盘	每帧 `ofstream << ... << std::endl`
地图保存	暂停建图或复制快照后保存	在地图被优化线程修改时直接序列化
异常处理	I/O 层抛出带上下文的异常，上层决定退出或降级	捕获后只打印 `"error"`
临时文件	写到 `.tmp`，成功后 `rename`	直接覆盖原地图文件

std::endl 会强制 flush，这在日志少时无所谓，在高频轨迹输出中会放大磁盘延迟。需要实时安全时，用 '\n' 缓冲输出，后台线程定期 flush。

练习¶

读写往返题：实现 saveTum() 和 loadTum()，随机生成 100 个单位四元数位姿，写入文件后再读回，要求平移误差小于 \(10^{-9}\)、旋转角误差小于 \(10^{-9}\) rad。
边界题：构造 5 个坏文件：缺列、多列、四元数未归一、时间戳倒退、科学计数法时间戳。要求解析器分别给出不同错误信息。
跨章综合题：用通用库·李群manif 的 SE3d 表示轨迹，读取 TUM 文件后保存为 .g2o 顶点文件，再用 SLAM库·g2o 的 g2o 读取函数验证顶点数量和位姿误差。

本章小结¶

主题	核心工具	SLAM 应用	关键注意点
文件流基础	`ifstream`/`ofstream`	所有文件读写的基础	RAII 自动关闭，`\n` 优于 `endl`
KITTI/TUM 格式	`getline` + `stringstream`	数据集加载与轨迹保存	行优先矩阵、四元数顺序、精度控制
stringstream	`istringstream`/`ostringstream`	解析复杂文本格式	复用时需 `clear()`，性能敏感场景用 `from_chars`
string 操作	`find`/`substr`/`stod`	路径处理、配置解析	`string_view` 零拷贝但需注意生命周期
g2o 文件	tag 分支 + `>>` 解析	图优化数据 I/O	信息矩阵上三角元素数 \(n(n+1)/2\)
二进制 I/O	`read`/`write` + `reinterpret_cast`	点云、深度图	始终用 `ios::binary`，只对 POD 类型直接 I/O
fmt 格式化	`fmt::format`/`fmt::print`	日志、文件名生成	编译期类型检查，比 iostream 快 5-10 倍
filesystem	`path`/`directory_iterator`	数据集目录遍历	遍历顺序不确定，需显式排序
序列化框架	Boost/Protobuf/JSON/cereal	地图持久化	按数据规模和跨语言需求选择

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

Mini-LIO 项目进度：本章为 Mini-LIO 项目搭建数据 I/O 基础设施。

本章新增：io 模块

mini_lio/
├── include/mini_lio/
│   ├── io/
│   │   ├── kitti_reader.h       // KITTI 时间戳和位姿读取
│   │   ├── tum_io.h             // TUM 格式轨迹读写
│   │   ├── pointcloud_io.h      // 二进制点云读写
│   │   └── g2o_io.h             // g2o 图文件读写
│   └── ...
├── src/io/
│   ├── kitti_reader.cpp
│   ├── tum_io.cpp
│   ├── pointcloud_io.cpp
│   └── g2o_io.cpp
└── test/
    └── test_io.cpp              // 读写往返测试

实现要求：

KITTIReader：读取 times.txt 和 poses.txt，提供按帧号索引的接口。
saveTUM / loadTUM：TUM 格式轨迹的读写，时间戳精度 9 位小数。
readBinCloud / writeBinCloud：KITTI .bin 格式点云的读写。
所有函数都应有错误处理（文件不存在、格式错误）和单元测试（写入后读取验证数据一致性）。

与后续章节的衔接：通用库·Eigen（Eigen）将使用 KITTIReader 加载位姿数据并转换为 Eigen::Isometry3d。通用库·李群manif（Sophus）将在 tum_io 的基础上添加 SE3 轨迹保存。SLAM库·g2o（g2o）将使用 g2o_io 读取优化前后的图结构。

延伸阅读¶

资源	内容	难度
The C++ Programming Language 并发与系统编程/CUDA在SLAM中的应用 (Stroustrup)	I/O 流的设计哲学与实现细节	⭐⭐⭐
fmt 库文档	格式化语法参考、性能基准	⭐⭐
cppreference: filesystem	`<filesystem>` 完整 API 参考	⭐⭐
KITTI 数据集官网	数据集格式规范和评测工具	⭐⭐
TUM RGB-D 数据集	TUM 格式定义和评测脚本	⭐⭐
evo 工具文档	轨迹评测工具的安装与使用	⭐⭐
Boost.Serialization 教程	侵入式与非侵入式序列化、版本兼容	⭐⭐⭐
Protocol Buffers 官方文档	.proto 语法、编译流程、性能优化	⭐⭐⭐
nlohmann/json GitHub	API 参考、性能对比、高级用法	⭐⭐
slambook2 第 2 章（高翔）	SLAM 数据集加载的入门示例	⭐⭐
PCL PCD 格式规范	PCD 文件头、ASCII/二进制模式说明	⭐⭐

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
`evo_ape` 报轨迹格式错误	TUM/KITTI 列数不对、时间戳被截断、行尾混入非数值字段	1. 用 `awk '{print NF}'` 检查列数；2. 打印第一行和报错行；3. 用本章解析器逐字段报错	通用库·文件IO, 通用库·李群manif
轨迹能读取但评测误差极大	四元数构造顺序错，把 `qx qy qz qw` 当成 `qw qx qy qz`	1. 检查 Eigen 构造函数是否用 `Quaterniond(qw,qx,qy,qz)`；2. 验证旋转矩阵行列式接近 1；3. 用单位四元数测试	通用库·文件IO, 通用库·李群manif
二进制点云读取后坐标全是极大值或 NaN	未用 `ios::binary`、字段顺序不一致、大小端或 `float/double` 混用	1. 检查文件大小是否为点数 × 单点字节数；2. 打印前 3 个点原始字节；3. 明确每个字段类型	通用库·文件IO, 通用库·PCL
数据集帧顺序每次运行不同	`directory_iterator` 遍历顺序未排序	1. 遍历后对路径排序；2. 单元测试比较两次扫描结果；3. 不依赖文件系统返回顺序	通用库·文件IO
地图保存一半后程序退出，下一次无法加载	直接覆盖正式文件，保存过程中被中断	1. 先写 `.tmp` 文件；2. 写完 flush 并关闭；3. 成功后原子 rename 替换正式文件	通用库·文件IO