基础类型与数学工具

基础类型与数学工具 模块是 EROAM 系统的数值计算基础设施层，它为整个 SLAM 系统提供统一的类型定义、坐标转换和数学运算工具。该模块的核心价值在于消除重复的类型定义和数学实现，确保从传感器数据到优化求解器的整个数据流中，类型一致性和数值精度得到保障。对于刚加入团队的工程师，理解这个模块是理解整个系统数据流的关键起点。

Overview

问题空间与设计动机

在 LiDAR SLAM 系统中，数据会流经多个处理阶段：原始点云采集、特征提取、位姿估计、地图更新、回环检测等。每个阶段都可能使用不同的数值类型（float vs double）、不同的坐标表示（Eigen 向量 vs PCL 点云 vs Sophus 位姿），以及不同的数学运算（几何变换、概率分布、线性代数）。

如果没有统一的基础设施，会出现以下问题：

1. 类型碎片化：不同子系统各自定义 Vec3d、Point3D 等相似类型，导致接口转换代码泛滥
2. 精度不一致：部分模块用 float 求速度，部分用 double 求精度，混合运算时产生隐式转换开销和精度损失
3. 数学工具重复：每个模块都实现自己的高斯分布计算、位姿插值、平面拟合等函数
4. 内存对齐问题：PCL 点云要求特定的内存对齐，自定义类型若未正确处理会导致段错误

该模块的设计哲学是**"一次定义，全局使用"**——在系统最底层提供经过仔细设计的类型别名和数学工具，上层模块只需包含头文件即可获得一致的数值计算能力。

架构概览

模块由三个核心头文件组成：

核心设计决策

1. 类型别名 vs 新类型封装

模块大量使用 using 别名（如 using Vec3d = Eigen::Vector3d）而非封装类。这是有意为之：

• 优点：零开销，与 Eigen API 完全兼容，可直接使用 Eigen 的全部功能
• 代价：无法添加约束或不变式（如单位四元数检查）

对于需要额外语义保证的场景（如位姿），模块选择依赖 Sophus 的 SE3 类型而非自己封装，避免重复造轮子。

2. 模板化 vs 具体类型

数学工具（如 Exp, GaussianPDF）使用模板实现，支持 float 和 double：

// src/math_utils.h:219-224
template<typename T, int N = 2>
inline double GaussianPDF(const Eigen::Matrix<T, N, 1> &mean, 
                          const Eigen::Matrix<T, N, N> &cov,
                          const Eigen::Matrix<T, N, 1> &x)

这种设计允许同一套代码服务于实时前端（可能用 float 求速度）和精确后端（用 double 求精度）。但模板也带来了编译时间增加和错误信息冗长的问题。

3. 头文件-only 实现

所有组件都是头文件-only（header-only），没有对应的 .cpp 实现文件。这简化了构建配置，但意味着：

• 模板实例化发生在每个编译单元，可能增加二进制体积
• 修改头文件会触发依赖它的全部代码重新编译
• 没有隐藏实现细节的可能，所有实现都是公开的

子模块详解

eigen_types.h —— 类型别名与哈希基础设施

该文件是系统的类型契约层。它定义了所有数值计算的基础词汇表。

核心类型体系

向量类型：从 Vec2i（2D 整数向量）到 Vec18d（18维双精度向量），覆盖从图像坐标到高维状态向量的需求。

矩阵类型：命名遵循 Mat{rows}d / Mat{rows}{cols}d 的约定，如 Mat23d 表示 2x3 双精度矩阵。动态维度使用 MatXd 和 VecXd。

位姿类型：通过 Sophus 库引入 SE2/SE3（特殊欧氏群，表示刚体位姿）和 SO2/SO3（特殊正交群，表示纯旋转）。

向量哈希：空间索引的关键

// src/eigen_types.h:75-78
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);
}

这个哈希函数实现了论文《Optimized Spatial Hashing for Collision Detection of Deformable Objects》中的空间哈希算法。它用于将 3D 整数坐标映射到哈希表索引，是体素地图（voxel map）和空间哈希数据结构的基础。

关键特性：

• 使用大质数（73856093, 471943, 83492791）作为乘子，确保哈希值在空间中均匀分布
• 取模 10000000 限制哈希值范围，适用于固定大小的哈希表
• 专门针对整数坐标设计，假设输入是离散化的体素索引

Mat4ToSE3：矩阵到位姿的鲁棒转换

// src/eigen_types.h:112-119
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>());
}

这个函数解决了一个实际问题：从 4x4 变换矩阵中提取位姿时，旋转部分可能由于数值误差不再是严格正交矩阵。Sophus 的 SE3 构造函数会检查旋转矩阵的正交性，失败时抛出异常。

设计权衡：

• 选择四元数作为中间表示，因为归一化四元数总是对应有效旋转
• 强制转换为 double 进行归一化，避免精度损失
• 注释中明确说明这是为了"prevent sophus check failure"

math_utils.h —— 数学运算工具箱

该文件是系统的数值计算引擎，包含从基础几何到 SLAM 专用算法的广泛功能。

SO(3) 指数映射与对数映射

// src/math_utils.h:251-262
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;
    }
}

这是罗德里格斯旋转公式（Rodrigues' Rotation Formula）的实现，将旋转向量（李代数 so(3)）转换为旋转矩阵（李群 SO(3)）。

数值稳定性考虑：

• 小角度阈值 0.0000001 用于避免除以零
• 当角度极小时直接返回单位矩阵（一阶近似足够精确）
• 存在多个重载版本，支持不同输入形式（向量、分量、角速度×时间）

边缘化（Marginalization）：舒尔补的实现

// src/math_utils.h:589-662
inline Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end)

这是 SLAM 后端优化中的核心操作。在滑动窗口优化中，当旧的状态变量需要被移除但又要保留它们对剩余变量的约束时，使用边缘化。

算法流程：

1. 计算信息矩阵 H 的舒尔补（Schur complement）
2. 使用 SVD 处理可能的数值奇异问题（singularValues_inv(i) > 1e-6 检查）
3. 通过行列重排保持矩阵结构

关键代码片段：

// 数值稳定性：小奇异值设为 0 而非取倒数
for (int i = 0; i < b; ++i) {
    if (singularValues_inv(i) > 1e-6) 
        singularValues_inv(i) = 1.0 / singularValues_inv(i);
    else
        singularValues_inv(i) = 0;
}

高斯分布操作

模块提供了多种高斯分布相关操作：

• GaussianPDF：计算多元高斯分布的概率密度函数，用于概率数据关联
• UpdateMeanAndCov：合并两个高斯分布（贝叶斯更新）
• HistoryMeanAndVar：增量式更新均值和方差，适用于在线统计

平面估计：动态矩阵版本

// src/math_utils.h:380-407
template<typename T>
inline bool esti_plane_dynamic(Eigen::Matrix<T, 4, 1> &abcd, 
                               const std::vector<Vec3d> &point, 
                               const double &threshold)

使用 colPivHouseholderQr 求解平面方程 ax + by + cz + d = 0。选择 QR 分解而非 SVD 是因为：

• 对于平面拟合（3 未知数），QR 分解比 SVD 更快
• colPivHouseholderQr 提供良好的数值稳定性

注释中的警告（来自代码中的中文注释）：

// Marked by xiaotao:
// Do not change the types of matrices A and b (including double and Eigen::Dynamic), 
// issues may arise when points.size() is 3 or 4, reason unknown.

这是一个典型的经验性约束——当点数很少（3 或 4 个）时，矩阵类型会影响 Eigen 的求解路径，可能导致数值问题。

point_types.h —— 点云类型与转换

该文件是系统的数据接口层，处理 PCL（Point Cloud Library）点类型与 Eigen 向量之间的转换。

基础点类型

// src/point_types.h:16-17
using PointType = pcl::PointXYZI;  // 带强度的 3D 点
using PointCloudType = pcl::PointCloud<PointType>;
using CloudPtr = PointCloudType::Ptr;

PointXYZI 是系统的基础点类型，包含 x, y, z, intensity 四个字段。选择它作为默认类型是因为：

• 兼容大多数 LiDAR 数据（Velodyne, Ouster, Livox 等）
• 强度信息对特征提取（如边缘检测）有用
• PCL 对其有优化的处理函数

FullPointType：扩展点属性

// src/point_types.h:47-59
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
};

FullPointType 用于需要完整 LiDAR 原始信息的场景（如运动补偿、特征提取）。关键设计：

• EIGEN_MAKE_ALIGNED_OPERATOR_NEW：确保包含 Eigen 成员的结构体正确对齐（16 字节边界）
• PCL 宏 POINT_CLOUD_REGISTER_POINT_STRUCT：向 PCL 注册自定义点类型，使其可用于 PCL 算法

类型转换函数

// src/point_types.h:20-21
inline Vec3f ToVec3f(const PointType& pt) { 
    return pt.getVector3fMap(); 
}
inline Vec3d ToVec3d(const PointType& pt) { 
    return pt.getVector3fMap().cast<double>(); 
}

getVector3fMap() 返回 Eigen 的 Map 对象，零开销地映射 PCL 点的内存布局到 Eigen 向量。.cast<double>() 显式转换精度。

关键设计权衡

1. 精度选择：float vs double

模块通过模板支持两者，但默认使用 double（如 Vec3d 是优先选择的向量类型）。

2. 动态矩阵 vs 静态矩阵

math_utils.h 中同时存在：

• 静态大小矩阵实现：如 FitPlane 使用固定大小矩阵接口，编译期检查维度
• 动态大小矩阵实现：如 esti_plane_dynamic 使用 Eigen::Dynamic 维度，运行时确定大小

权衡理由：

• 静态矩阵：更快（栈分配、编译期优化），适用于维度已知的场景（3x3旋转矩阵、6x6位姿协方差）
• 动态矩阵：更灵活，适用于维度随输入变化的场景（滑动窗口大小可变的信息矩阵）

C++ 实现细节

1. 内存所有权模型

• 所有类型均为值语义：本模块没有自定义的动态内存分配，所有返回值都是栈分配对象，内存由调用者自动管理
• Eigen::Map 零拷贝转换：ToVec3f/ToVec3d 等函数返回的 Eigen::Map 对象是 PCL 点内存的非拥有引用，调用者必须保证原 PCL 点云对象在 Map 使用期间有效
• PCL 智能指针：CloudPtr/FullCloudPtr 是 std::shared_ptr 的别名，使用引用计数管理点云内存，自动释放
• 没有原始 new/delete：所有内存管理均由标准库和第三方库（Eigen, PCL, Sophus）自动处理，避免手动内存管理错误

2. 对象生命周期与值语义

• 所有类型遵循 Rule of Zero：自定义类型（如 FullPointType）仅包含普通值成员，编译器生成的拷贝、移动、析构函数完全正确
• 点云拷贝 vs 移动：PointCloudType 的拷贝是深拷贝（复制所有点数据），而移动是零开销的所有权转移，代码中优先使用移动语义传递点云
• 迭代器有效性：Eigen 矩阵和 PCL 点云的 resize 操作会使所有现有迭代器、指针和引用失效，这一点在上层代码中需要特别注意
• 临时对象生命周期扩展：所有返回 const& 的函数都是引用参数的转发，没有返回临时对象引用的情况，避免悬垂引用

3. 错误处理策略

• 两层错误检查：
  1. 调试期：大量使用 assert 检查前置条件（如输入点数量 >=3，矩阵维度匹配），断言失败直接终止程序
  2. 运行期：边界条件检查（如 GetPixelValue 中的坐标裁剪）和返回值状态（如平面拟合失败返回 false）
• 异常安全：所有函数均提供基本异常安全保证（无内存泄漏），大部分数学运算函数是 noexcept（依赖 Eigen 的异常安全保证）
• 错误传播：错误要么通过返回 false 直接返回给调用者，要么通过 LOG(ERROR) 记录日志，不会在模块内部吞掉错误
• 未定义行为风险点：
  • 传递空的点云给平面/直线拟合函数（仅调试期有 assert 检查，发布期无检查）
  • 传递超过范围的索引给 Marginalize 函数（无运行时检查）

4. Const 正确性

• 所有纯计算函数（如 GaussianPDF, Exp, ToVec3d）的输入参数均为 const&，不会修改输入
• 没有 mutable 成员，所有类型的 const 方法都是真正的无副作用
• 输出参数均为指针或非 const 引用，明确区分输入输出：
  • const T* 或 const T&：纯输入
  • T* 或 T&：输出或输入输出

5. API 契约与前置条件

6. 并发与线程安全

• 所有函数都是纯函数：没有全局或静态可变状态，相同输入总是产生相同输出
• 线程安全：所有函数都可以安全地从多个线程并发调用，不需要外部同步
• 没有同步原语：模块本身不使用互斥量、原子变量等，因为没有共享可变状态
• 注意：传递给函数的非 const 输入对象（如 std::map）如果被多个线程并发修改，调用者需要负责同步

7. 性能优化点

• 热点路径：
  • Exp/Log 函数：在位姿优化中每帧被调用数千次，使用分支预测友好的小角度特殊处理
  • esti_plane_dynamic：点云特征提取时频繁调用，使用快速的 QR 分解而非 SVD
  • ToVec3d/ToPointType：每帧被调用数百万次，实现为 inline 函数，零开销转换
• 数据布局：
  • FullPointType 字段按 4 字节边界对齐，与 PCL 点的内存布局兼容，避免 padding 开销
  • Eigen 矩阵默认列优先存储，与 Sophus 和 PCL 的存储顺序一致，不需要转换
• 算法复杂度：
  • 平面/直线拟合：O(n)，n 为点数，适合小批量点（<100 个）的拟合
  • 边缘化：O(b³)，b 为边缘化的变量数量，适合小窗口滑动（b<20）
  • 位姿插值：O(n)，n 为姿态数量，实际使用中通常是二分查找优化到 O(log n)

与其他模块的依赖关系

本模块是系统的底层基础设施，没有内部依赖，仅依赖第三方库：

• Eigen：线性代数基础
• Sophus：李群/位姿操作
• PCL：点云类型定义
• OpenCV：图像像素操作

下游模块均依赖本模块：

• 点云与索引工具：使用 PointType 和几何变换函数
• KD树结构与距离节点：使用点类型和距离计算函数
• EROAM 核心算法：使用全部数学工具和类型定义

新开发者注意事项

常见陷阱

1. 不要自定义新的向量/矩阵类型：优先使用 eigen_types.h 中已定义的别名，避免类型碎片化
2. 不要修改 esti_plane_dynamic 中的矩阵类型：如注释所述，修改为静态矩阵会导致点数为 3/4 时的数值问题
3. 注意 Eigen 对齐问题：包含 Eigen 成员的自定义类必须添加 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 宏，否则在 ARM/x86 平台上可能出现段错误
4. 角度单位约定：所有数学函数均使用弧度作为输入输出，角度转弧度请使用 deg2rad 函数，不要手动计算
5. 高斯 PDF 没有归一化常数：GaussianPDF 函数返回的是未归一化的概率密度，仅适合相对比较，不适合直接作为概率使用

扩展指南

• 添加新的数学函数时，优先使用模板实现，支持 float 和 double 两种精度
• 添加新的点类型时，必须使用 POINT_CLOUD_REGISTER_POINT_STRUCT 宏向 PCL 注册
• 涉及数值稳定性的修改必须添加单元测试，测试边界情况（小角度、小行列式、接近奇异的矩阵）
• 所有公共函数必须添加注释，说明前置条件、返回值语义和使用场景

调试建议

• 出现 NaN 数值时，首先检查 GaussianPDF 或 Marginalize 的输入矩阵是否半正定
• 段错误如果发生在 Eigen 操作时，优先检查内存对齐问题，再检查矩阵维度是否匹配
• 平面拟合失败时，检查输入点是否共线，或阈值设置是否过严

API 索引