数值基础设施：类型系统与数学工具

SLAM 系统的数据流像一条河流。上游是传感器输出的原始点云，中游经过特征提取与位姿估计，下游汇入优化求解器。如果每个河段都使用不同的"容器"来盛装数据——有的用 PCL 点云，有的用 Eigen 向量，有的用自定义结构体——那么工程师将在无尽的类型转换中消耗精力，而系统则会在隐式的精度损失中积累误差。

EROAM 的数值基础设施层，就是为了让这条河流使用统一的度量衡而设计的。

类型碎片化：一个真实的问题

想象你正在调试一个位姿估计异常。追踪数据流向，你发现：

• 激光雷达驱动输出 pcl::PointXYZI
• 前端特征提取转换为 Eigen::Vector3f 求速度
• 后端优化器要求 Eigen::Vector3d 保证精度
• 可视化模块又转回 pcl::PointXYZRGB

每个转换点都是潜在的 bug 来源：float 到 double 的隐式转换可能放大数值误差，Eigen 到 PCL 的内存布局差异可能导致对齐错误，而分散在各处的 Point3D、Vec3、Vector3d 类型定义让代码搜索变得困难。

EROAM 的解决方案是在系统最底层建立单一的类型词汇表。

核心类型体系

eigen_types.h：类型契约层

这个头文件定义了 EROAM 的数值方言。所有模块都通过这些别名进行通信，而非直接使用底层库的类型。

// 向量类型：从 2D 到 18D，整数与浮点
using Vec2i = Eigen::Matrix<int, 2, 1>;
using Vec3d = Eigen::Vector3d;           // 最常用：双精度 3D 点
using Vec18d = Eigen::Matrix<double, 18, 1>;  // 高维状态向量

// 位姿类型：来自 Sophus 库
using SE3 = Sophus::SE3d;   // 刚体位姿（旋转+平移）
using SO3 = Sophus::SO3d;   // 纯旋转

选择 using 别名而非封装类，意味着零运行时开销和与 Eigen API 的完全兼容。代价是无法在编译期强制执行某些不变式（如四元数的单位长度）。对于需要这种保证的场景，EROAM 选择依赖 Sophus 的成熟实现，而非自己封装。

空间哈希：体素地图的基础设施

template<>
inline size_t hash_vec<3>::operator()(const Eigen::Matrix<int, 3, 1> &v) const {
    return size_t(((v[0] * 73856093) ^ (v[1] * 471943) ^ (v[2] * 83492791)) % 10000000);
}

这个函数实现了空间哈希算法，将 3D 整数坐标映射到哈希表索引。三个大质数乘子（73856093, 471943, 83492791）确保哈希值在空间中均匀分布，避免聚类冲突。它是体素地图和哈希空间索引的基础组件。

Mat4ToSE3：数值鲁棒性的典范

从 4x4 变换矩阵提取位姿时，旋转部分可能因数值误差偏离严格正交。Sophus 的构造函数会检查这一点，失败时抛出异常。

template<typename S>
inline SE3 Mat4ToSE3(const Eigen::Matrix<S, 4, 4> &m) {
    Quatd q(m.template block<3, 3>(0, 0).template cast<double>());
    q.normalize();  // 关键：防止 Sophus 检查失败
    return SE3(q, m.template block<3, 1>(0, 3).template cast<double>());
}

设计决策很清晰：通过四元数作为中间表示，归一化操作总能产生有效旋转。强制使用 double 精度进行这一关键步骤，避免精度损失累积。

数学工具箱：从几何到优化

math_utils.h 是系统的数值计算引擎。它的设计遵循一个原则：同一套代码服务于实时前端和精确后端。

SO(3) 指数映射：罗德里格斯公式

template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang) {
    T ang_norm = ang.norm();
    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
    if (ang_norm > 0.0000001) {
        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
        Eigen::Matrix<T, 3, 3> K = SKEW_SYM_MATRIX(r_axis);
        return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
    } else {
        return Eye3;
    }
}

这是李群与李代数之间的桥梁。旋转向量（李代数 so(3)）通过罗德里格斯公式转换为旋转矩阵（李群 SO(3)）。小角度阈值 0.0000001 避免除以零；当角度极小时，一阶近似（单位矩阵）已足够精确。

模板实现同时支持 float 和 double，让前端速度优化和后端精度保证共享同一套代码。

边缘化：滑动窗口优化的核心

Marginalize 函数实现这一关键操作。当滑动窗口需要丢弃旧帧但保留其信息时，它通过舒尔补计算边缘化后的信息矩阵。数值稳定性通过 SVD 实现：小于 1e-6 的奇异值被置零而非取倒数，避免放大数值噪声。

平面估计：速度与精度的权衡

template<typename T>
inline bool esti_plane_dynamic(Eigen::Matrix<T, 4, 1> &abcd, 
                               const std::vector<Vec3d> &point, 
                               const double &threshold)

使用 colPivHouseholderQr 而非 SVD，因为对于 3 个未知数的平面拟合，QR 分解更快且足够稳定。代码中的中文注释是一个重要警告：

"Do not change the types of matrices A and b... issues may arise when points.size() is 3 or 4"

这是经验性的约束：当点数很少时，Eigen 的求解路径对矩阵类型敏感，修改可能导致数值问题。

点云类型：PCL 与 Eigen 的桥梁

point_types.h 处理一个具体需求：PCL 点云库的类型与 Eigen 向量之间的零开销转换。

基础点类型

using PointType = pcl::PointXYZI;        // 带强度的 3D 点
using PointCloudType = pcl::PointCloud<PointType>;
using CloudPtr = PointCloudType::Ptr;    // shared_ptr 别名

PointXYZI 是系统默认的点类型，兼容主流 LiDAR（Velodyne, Ouster, Livox）。强度信息对特征提取至关重要。

FullPointType：完整原始信息

struct FullPointType {
    PCL_ADD_POINT4D;      // x, y, z, padding
    float range = 0;      // 距离
    float radius = 0;     // 水平距离
    uint8_t intensity = 0;
    uint8_t ring = 0;     // 激光线束编号
    uint8_t angle = 0;    // 水平角度
    double time = 0;      // 时间戳（运动补偿关键）
    float height = 0;     // 相对高度
    
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // 16字节对齐保证
};

EIGEN_MAKE_ALIGNED_OPERATOR_NEW 宏至关重要。包含 Eigen 成员的结构体必须正确对齐到 16 字节边界，否则在 SIMD 指令集上会产生段错误。

零拷贝转换

inline Vec3f ToVec3f(const PointType& pt) { 
    return pt.getVector3fMap(); 
}
inline Vec3d ToVec3d(const PointType& pt) { 
    return pt.getVector3fMap().cast<double>(); 
}

getVector3fMap() 返回 Eigen::Map 对象，直接映射 PCL 点的内存布局，没有数据拷贝。.cast<double>() 显式转换精度，避免隐式转换的意外行为。

设计权衡与工程决策

精度选择矩阵

模板支持两者，但默认优先 double（Vec3d 是首选向量类型）。

头文件-only 的代价与收益

所有组件都是头文件-only，没有 .cpp 实现文件。

收益：简化构建配置，模板实例化灵活。

代价：编译时间增加，修改触发大范围重编译，实现细节完全公开。

EROAM 接受这一权衡，因为数值基础设施的稳定性高于迭代频率——这些文件一旦成熟，很少需要修改。

静态 vs 动态矩阵

• 静态矩阵（Mat3d）：栈分配，编译期优化，用于已知维度（3x3 旋转、6x6 位姿协方差）
• 动态矩阵（MatXd）：运行时确定大小，用于滑动窗口等可变维度场景

数据流全景：类型如何贯穿系统

这个架构确保：从 ROS 消息进入系统的那一刻起，到最终优化输出，所有数值计算使用统一的类型词汇表。没有隐式转换的陷阱，没有内存对齐的意外，没有重复实现的数学函数。

新开发者的实践指南

该做的

• 优先使用 eigen_types.h 中已定义的别名，不要创建新的向量类型
• 包含 Eigen 成员的自定义类必须添加 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
• 角度输入使用弧度，通过 deg2rad 函数转换，不要手动计算
• 涉及数值稳定性的修改必须添加单元测试，覆盖小角度、小行列式、接近奇异矩阵等边界情况

不该做的

• 不要修改 esti_plane_dynamic 中的矩阵类型（静态/动态维度），点数 3/4 时会出现数值问题
• 不要假设 GaussianPDF 返回归一化概率，它仅适合相对比较
• 不要在多个线程间无保护地修改传递给数学函数的容器（函数本身是纯函数，但输入对象不是）

调试线索

关键 API 速查

这些类型和函数构成了 EROAM 的数值基础设施。理解它们的设计哲学——统一而非多样，显式而非隐式，零开销而非便利——是掌握整个系统数据流的关键起点。