📚 课程目录第3章 3D激光SLAM数据表示

3、3D激光SLAM数据表示：点云、位姿、变换矩阵、四元数、李群李代数基础，以及噪声模型

3D激光SLAM的后端优化本质上是“在状态空间中寻找最优轨迹与地图”的过程，而这一切的起点，是精确且统一的数据表示。如果数据表示不一致或坐标系混乱，再强大的优化器也无法得到正确结果。本章将系统梳理3D激光SLAM中最核心的数据载体：点云、位姿、变换矩阵、四元数以及李群李代数，并深入讨论噪声模型如何与优化框架衔接。这些概念不仅是g2o与GTSAM的输入接口，更是理解残差构建、雅可比推导和鲁棒核函数作用的前提。

上图展示了3D激光SLAM中数据表示的完整链路。从传感器原始点云出发，经过特征提取与配准，形成位姿图节点与边，最终由g2o或GTSAM进行优化。其中，变换矩阵、四元数与李代数分别承担了不同的数学职责：变换矩阵用于坐标转换，四元数用于紧凑且无奇异的旋转存储，李代数则用于优化过程中的求导与增量更新。理解这三者的关系与适用场景，是掌握后端优化的第一道门槛。

3.1 点云数据表示与坐标系约定

核心原理

3D激光雷达（如Velodyne VLP-16、Ouster OS1-64、Livox Horizon）输出的原始数据通常以点云（Point Cloud）形式存在。每个点包含三维坐标 (x, y, z) 以及附加属性（强度intensity、环号ring、时间戳timestamp）。在SLAM系统中，点云必须被转换到统一的坐标系下才能进行配准与建图。常见的坐标系包括：

雷达坐标系（LiDAR Frame）：以激光雷达中心为原点，x轴向前，y轴向左，z轴向上（右手系）。
机体坐标系（Body Frame）：以IMU或车辆中心为原点，通常与雷达坐标系存在固定的外参变换。
世界坐标系（World Frame）：SLAM系统的全局参考系，通常以起始时刻的机体坐标系为原点。

点云数据的质量直接影响配准精度。例如，VLP-16的水平角分辨率为0.2°，垂直视场角为±15°，在20米处相邻点间距约为7cm。这种稀疏性要求配准算法（如ICP、NDT）对初始位姿敏感，因此后端优化必须提供良好的初始值。

操作流程：点云预处理与坐标变换

去畸变：对于旋转式雷达，每个点的采集时刻不同，需利用IMU或匀速运动模型将点云补偿到同一时刻。
外参变换：将雷达坐标系下的点云通过外参矩阵 T_BL 变换到机体坐标系：p_B = T_BL · p_L。
体素滤波：下采样以减少计算量，例如使用0.1m体素栅格。
地面分割与聚类：去除地面点，提取特征点（边缘点、平面点）。

  实战要点：在g2o或GTSAM中，点云本身并不直接作为优化变量。优化的是位姿节点（即雷达在世界坐标系下的位置 TWL）。点云通过配准算法（如ICP）产生相邻帧之间的相对位姿约束，这些约束构成因子图中的边。

案例拆解：VLP-16点云坐标变换

假设VLP-16安装于车辆顶部，外参为：平移 (0.5, 0, 1.2) 米，旋转为绕z轴旋转90°（即雷达x轴指向车辆左侧）。则外参矩阵 T_BL 为：

T_BL = [ 0 -1  0  0.5 ]
       [ 1  0  0  0   ]
       [ 0  0  1  1.2 ]
       [ 0  0  0  1   ]

若雷达检测到前方3米处有一个点 p_L = (3, 0, 0)，则变换到机体坐标系：

p_B = T_BL * p_L = (0, 3, 1.2)

这意味着该点在车辆左侧3米、高度1.2米处。若忽略外参标定误差（如旋转角偏差0.5°），在50米处将产生约0.44米的横向误差，足以导致建图重影。

常见误区与总结

误区1：将雷达坐标系直接当作世界坐标系使用，忽略外参标定。
误区2：点云去畸变时使用线性插值，但在高速旋转或剧烈运动时误差较大，应使用IMU预积分或B样条插值。
误区3：体素滤波尺寸过大导致特征丢失，过小则计算量剧增。建议根据雷达线束密度动态调整。

3.2 位姿表示：变换矩阵与四元数

核心原理

位姿（Pose）描述物体在三维空间中的位置与朝向。在SLAM中，位姿通常表示为齐次变换矩阵 T ∈ SE(3)：

T = | R  t |
    | 0  1 |

其中 R ∈ SO(3) 是旋转矩阵，t ∈ R³ 是平移向量。旋转矩阵有9个参数但只有3个自由度，存在冗余约束（正交且行列式为1）。直接使用旋转矩阵作为优化变量会导致优化过程破坏其约束，因此需要更紧凑的表示。

四元数是一种超复数表示，q = (w, x, y, z)，满足 w² + x² + y² + z² = 1。四元数避免了欧拉角的万向锁问题，且存储仅需4个double（比旋转矩阵少5个），是g2o和GTSAM中旋转的标准存储格式。例如，绕z轴旋转90°的四元数为 q = (cos45°, 0, 0, sin45°) ≈ (0.707, 0, 0, 0.707)。

判断方法：何时使用四元数 vs 变换矩阵

维度	变换矩阵 T	四元数 q + t
自由度	6 (R:3 + t:3)	7 (q:4 + t:3) 带约束
冗余性	旋转矩阵9参数冗余	四元数4参数冗余1个
插值	不直观	球面线性插值(SLERP)
优化适用性	需强制正交约束	需归一化处理
g2o/GTSAM内部	用于坐标变换	用于存储与更新

在g2o中，顶点类型 VertexSE3 内部使用四元数+平移向量存储，但对外提供变换矩阵接口。GTSAM的 Pose3 类同样如此。优化时，增量更新采用李代数扰动，而不是直接对四元数做加法。

案例拆解：四元数归一化与姿态插值

假设两个关键帧位姿：

帧1: q1 = (0.999, 0.01, 0.02, 0.03), t1 = (0, 0, 0)
帧2: q2 = (0.998, 0.04, 0.05, 0.06), t2 = (1.0, 0.2, 0.1)

若直接对四元数进行线性插值（LERP），会导致插值后的四元数不再满足单位范数，旋转速度也不均匀。正确做法是使用SLERP：

q_interp = sin((1-α)θ)/sinθ * q1 + sin(αθ)/sinθ * q2
其中 cosθ = q1·q2, α ∈ [0,1]

在g2o中，位姿的更新公式为：T_new = exp(δξ) · T_old，其中 δξ 是李代数增量。这保证了更新后的位姿始终在SE(3)流形上。

常见误区与总结

误区1：直接对四元数做减法或加法，导致范数不为1。应使用对数映射将四元数转换到李代数空间进行运算。
误区2：混淆旋转矩阵的左右乘规则。世界坐标系下的旋转用左乘，局部坐标系下的旋转用右乘。
误区3：忽略四元数的双覆盖特性（q 和 -q 表示同一旋转）。在优化中应保持符号一致性。

3.3 李群与李代数基础

核心原理

李群（Lie Group）是连续可微的群，SO(3) 和 SE(3) 是最典型的矩阵李群。李代数（Lie Algebra）是李群单位元处的切空间，SO(3) 对应 so(3)（三维向量 φ），SE(3) 对应 se(3)（六维向量 ξ = [ρ, φ]）。

李群与李代数通过指数映射和对数映射相互转换：

指数映射: exp(φ^) = R ∈ SO(3)
对数映射: log(R) = φ ∈ so(3)

其中 φ^ 是 φ 的反对称矩阵。对于SE(3)，指数映射为：

exp(ξ^) = | exp(φ^)  Jρ |
           |   0       1  |

J 是左雅可比矩阵。这一转换的核心价值在于：李代数是无约束的向量空间，可以在其上直接进行加法、求导和优化，而李群则保证变换的合法性。

操作流程：扰动模型与雅可比推导

左扰动模型：对位姿 T 施加微小扰动 δξ，则 T' = exp(δξ^) · T。残差关于 δξ 的雅可比为 ∂r/∂δξ。
右扰动模型：T' = T · exp(δξ^)，适用于局部坐标系下的误差。
链式法则：在因子图中，残差通常是观测值与预测值之差，需要分别对位姿和路标点求雅可比。

  g2o/GTSAM中的实现：g2o的 EdgeSE3 默认使用左扰动模型，雅可比矩阵在 linearizeOplus() 中计算。GTSAM则使用右扰动模型，通过 Pose3::Retract() 和 Pose3::LocalCoordinates() 实现。理解扰动模型是调试优化收敛性的关键。

案例拆解：基于李代数的位姿图优化

考虑两个位姿节点 T₁ 和 T₂，它们之间的相对测量为 ΔT₁₂。残差定义为：

r = log( ΔT_12^{-1} · T_1^{-1} · T_2 )

在g2o中，该残差的雅可比为：

∂r/∂δξ1 = -J_r^{-1} · Ad(T_1^{-1} · T_2)
∂r/∂δξ2 = J_r^{-1}

其中 Ad 是伴随变换，J_r 是右雅可比。若初始位姿误差较大（如平移误差超过1米），高斯牛顿法可能发散。此时应使用LM算法（Levenberg-Marquardt）并增加鲁棒核函数（如Huber核）来抑制异常边的影响。

常见误区与总结

误区1：将李代数直接当作普通向量进行加法，忽略了指数映射的非线性。应使用 exp 和 log 进行转换。
误区2：混淆左扰动与右扰动的雅可比形式，导致梯度方向错误。
误区3：在优化过程中不更新位姿的参考系，导致残差计算错误。

3.4 噪声模型与信息矩阵

核心原理

SLAM中的观测（如激光配准得到的相对位姿、IMU预积分）都带有噪声。噪声模型描述了观测误差的统计特性，通常假设为高斯分布：

e ~ N(0, Σ)

其中 Σ 是协方差矩阵。在因子图优化中，我们使用信息矩阵 Ω = Σ⁻¹ 作为残差的权重。优化目标为最小化马氏距离：

E = Σ_k e_k^T · Ω_k · e_k

对于激光SLAM，噪声主要来源于：

配准噪声：ICP/NDT配准的残差，与点云密度、几何结构有关。
运动畸变：雷达旋转期间车辆运动导致的点云变形。
外参标定误差：雷达与IMU之间的相对位姿不准确。

判断方法：如何设定信息矩阵

观测类型	协方差来源	典型值
ICP配准	配准得分、点云密度	平移: 0.01~0.05m, 旋转: 0.1~0.5°
IMU预积分	加速度计/陀螺仪噪声密度	根据IMU Allan方差标定
GPS/RTK	定位精度	RTK: 0.02m, 普通GPS: 1~3m
回环检测	匹配得分、特征数量	根据匹配质量动态调整

在实际工程中，信息矩阵通常被简化为对角矩阵，忽略不同维度间的相关性。但对于高精度建图，应使用完整的协方差矩阵。例如，激光雷达在水平方向的精度高于垂直方向，因此协方差矩阵中 x、y 方向的方差应小于 z 方向。

案例拆解：鲁棒核函数抑制错误回环

假设在长廊环境中，回环检测算法误匹配了两个相似但不同的位置，产生了一个错误的回环边。若使用标准高斯噪声模型，该错误边会以较大的权重参与优化，导致地图扭曲。

// g2o中的Huber核设置
auto robustKernel = new g2o::RobustKernelHuber();
robustKernel->setDelta(1.0); // 阈值1.0米
edge->setRobustKernel(robustKernel);

Huber核函数在残差小于阈值时使用二次函数，大于阈值时使用线性函数，从而降低异常值的影响。在GTSAM中，可以使用 HuberLoss 或 CauchyLoss。实际测试表明，在错误回环场景下，使用Huber核可将地图误差从2.3米降低到0.4米。

常见误区与总结

误区1：将信息矩阵设置为单位矩阵，忽略了不同传感器精度的差异。
误区2：鲁棒核阈值设置过大（如10米），导致核函数退化为二次函数，失去鲁棒性。
误区3：在优化过程中不更新信息矩阵，导致权重与当前状态不匹配。

噪声模型的正确建立是SLAM系统精度的关键。建议在工程实践中，通过Allan方差标定IMU噪声，通过配准残差统计标定激光噪声，并定期校验外参。在g2o和GTSAM中，信息矩阵的设定直接影响优化结果的置信度，需要结合传感器特性与场景特点反复调试。