3. 三维几何基础

点云配准的本质，是求解两个坐标系之间的刚体变换。无论是 ICP 迭代最近点、NDT 正态分布变换，还是基于特征的粗配准，最终都要回答同一个问题：把源点云旋转、平移到哪里，才能与目标点云重合？因此，理解三维几何中的位姿表示方法，是掌握配准与建图算法的前提。本章从刚体变换出发，依次梳理旋转矩阵、欧拉角、四元数、李群李代数，并落到坐标变换与位姿表示的工程实践上。

一、刚体变换：旋转与平移的统一描述

刚体变换指不改变物体形状和大小的空间变换，只包含旋转与平移两部分。三维空间中，一个点 p 经过刚体变换后变为：

p' = R · p + t
其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量

为把旋转和平移统一成一次矩阵乘法，引入齐次坐标，构造 4×4 变换矩阵 T：

T = [ R  t ]
    [ 0  1 ]

p'_h = T · p_h，p_h = [x, y, z, 1]^T

刚体变换有 6 个自由度：3 个旋转自由度加 3 个平移自由度。这 6 个自由度正是点云配准要求解的参数，也是位姿表示的核心。变换矩阵的逆为 T⁻¹ = [Rᵀ, -Rᵀt; 0, 1]，利用旋转矩阵正交性可快速求逆，这在配准中反复用到。

二、旋转矩阵、欧拉角与四元数

旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = 1。它无奇异性、可复合，但用 9 个参数表达 3 个自由度，存在冗余，且迭代优化时难以保持正交约束。

欧拉角用三个角度（如绕 Z-Y-X 的偏航 yaw、俯仰 pitch、滚转 roll）描述旋转，直观易读，但存在万向节死锁：当俯仰角为 ±90° 时，偏航与滚转退化，丢失一个自由度。因此欧拉角适合人机交互与显示，不适合作为优化变量。

四元数用 4 个参数 q = (w, x, y, z) 表示旋转，满足单位约束 ‖q‖ = 1。它无奇异性、插值平滑（SLERP），是 ROS、SLAM 系统中位姿存储与传输的主流格式。四元数与旋转矩阵可互相转换：

# 四元数转旋转矩阵（单位四元数）
R = [[1-2(y²+z²), 2(xy-wz),   2(xz+wy)],
     [2(xy+wz),   1-2(x²+z²), 2(yz-wx)],
     [2(xz-wy),   2(yz+wx),   1-2(x²+y²)]]

下表对比四种位姿表示方法的关键特性：

三、李群与李代数：优化视角下的位姿

旋转矩阵构成特殊正交群 SO(3)，刚体变换构成特殊欧氏群 SE(3)，它们都是李群——既是群又是光滑流形。李群上无法直接做加法，但可以通过指数映射 exp 与对数映射 log，在切空间（李代数）中做线性运算。

SO(3) 的李代数 so(3) 是三维向量 φ，对应反对称矩阵 φ^；SE(3) 的李代数 se(3) 是六维向量 ξ = (ρ, φ)，其中 ρ 为平移部分、φ 为旋转部分。指数映射把李代数元素映射回李群：

T = exp(ξ^) ≈ I + ξ^   （小量近似，用于迭代更新）

# 优化中位姿更新：
T_new = T_old · exp(δξ^)

在点云配准中，ICP 的误差函数对位姿求导时，正是利用李代数扰动模型：给当前位姿左乘或右乘一个微小扰动 exp(δξ^)，把非线性优化转化为对 6 维向量的线性最小二乘，再迭代更新。这就是 g2o、Ceres、GTSAM 等优化库处理位姿的底层逻辑。

四、坐标变换与位姿表示的工程实践

在多传感器建图中，点云往往来自不同坐标系：激光雷达坐标系、相机坐标系、IMU 坐标系、世界坐标系。位姿表示必须明确“谁在谁的坐标系下”。常用约定是 T_world_lidar 表示把雷达坐标系下的点变换到世界坐标系。变换链遵循矩阵乘法顺序：

p_world = T_world_lidar · T_lidar_camera · p_camera

# 注意：左乘表示在父坐标系中变换，右乘表示在自身坐标系中变换

下面用一张图梳理从点云到世界坐标的完整变换链与位姿表示选择：

总结来看，旋转矩阵适合变换复合，欧拉角适合显示，四元数适合存储与传输，李代数适合优化求解。点云配准的工程实现，通常以变换矩阵或四元数维护位姿，以 se(3) 李代数作为优化变量，最终把源点云经变换链投影到世界坐标系，完成配准与地图构建。理解这几种表示之间的转换关系与适用边界，才能在调试配准偏差时快速定位问题所在。