第一章 标定基础回顾：坐标系定义、刚体变换、李群李代数基础
各位同学，欢迎来到《传感器外参标定进阶训练》。我是你们这趟旅程的向导，一个在标定坑里摸爬滚打了好些年的工程师。
咱们直接开门见山。外参标定，说白了就是搞清楚「传感器A」相对于「传感器B」到底在哪儿、脸朝哪边。这事儿听起来简单，但一上手全是细节。今天这第一课，咱们先把地基夯实。我会带着大家把坐标系、刚体变换、还有那个听起来吓人的李群李代数，捋一遍。
嗯，别紧张。这些东西，你只要理解了背后的物理直觉，剩下的就是套公式。
1.1 坐标系：我们得先有个共同的「语言」
做标定，第一件事就是统一坐标系。你想想看，激光雷达说「前方5米」，相机说「像素(320, 240)」，这俩怎么对话？
我个人习惯，在项目一开始就画一张坐标系关系图。这能避免后期80%的混乱。
常见的坐标系就这几类：
- 世界坐标系 (W)：绝对的参考系。通常选在车体中心、或者第一帧的IMU位置。它是全局的锚点。
- 车体/机器人坐标系 (B)：固定在载体上。一般遵循右手系：X向前，Y向左，Z向上。这是运动控制的基准。
- 传感器坐标系 (S)：每个传感器自己的小世界。比如相机的光心、激光雷达的旋转中心。
避坑指南： 我曾经在一个项目里，把相机坐标系定义成了「Z向前」，而IMU是「X向前」。结果融合出来的轨迹直接画了个麻花。从那以后，我每次接手新项目，第一件事就是打印一份坐标系定义表，贴在工位上。
这里我画了一张图，帮你理清它们之间的关系：
我的小技巧： 在代码里，我习惯用 tf::StampedTransform 或者 geometry_msgs::TransformStamped 来管理这些关系。给每个坐标系一个唯一的名字，比如 base_link、velodyne、camera_color_optical_frame。这样ROS的tf树会自动帮你维护。
1.2 刚体变换：旋转 + 平移 = 外参
两个传感器之间的外参，就是一个刚体变换。它包含两部分：
- 旋转 (R)：传感器B的坐标轴，怎么转才能和传感器A对齐。
- 平移 (t)：传感器B的原点，相对于传感器A的原点，在哪儿。
数学上，我们写成：P_A = R * P_B + t
这里 P_B 是B坐标系下的一个点，P_A 是同一个点在A坐标系下的坐标。
旋转矩阵 R 有9个元素，但自由度只有3。所以实际工程里，我们很少直接用9个参数去优化。为什么？因为旋转矩阵必须是正交的（R^T * R = I），你直接优化9个数，很容易跑飞，得到一个非法的矩阵。
注意： 千万别直接用欧拉角做插值或优化！我见过太多人用欧拉角做优化，结果在接近90度时遇到万向锁，整个标定直接崩掉。欧拉角只适合给人看，不适合给机器算。
那用什么？四元数或者旋转向量。四元数紧凑、无奇异性，是工程中的首选。但四元数也有约束——它必须是单位四元数。这就引出了我们下一个话题：李群李代数。
1.3 李群与李代数：让优化变得优雅
说实话，我第一次接触李群李代数时，也觉得这玩意儿是数学家用来吓唬人的。但后来我发现，它解决了一个核心痛点：如何在流形上做优化。
你想想看，旋转矩阵 SO(3) 是一个流形，不是欧式空间。你不能直接在流形上做加减法。比如 R1 + R2 大概率不是一个合法的旋转矩阵。
李代数 so(3) 是流形在单位元处的切空间。它是一个向量空间。这意味着，你可以在李代数上做常规的加减、求导、优化。然后通过指数映射，把结果映射回流形上。
这个对应关系，我用一个表格总结一下：
| 概念 | 李群 (SO(3)) | 李代数 (so(3)) |
|---|---|---|
| 空间类型 | 流形（非欧式） | 向量空间（欧式） |
| 表示 | 旋转矩阵 R (3x3) | 旋转向量 φ (3x1) |
| 运算 | 乘法 | 加法 |
| 映射关系 | R = exp(φ^) | φ = log(R)^∨ |
| 优化方式 | 在流形上左乘/右乘扰动 | 在切空间直接加增量 |
在实际的标定代码里，我们通常用李代数来表示待优化的旋转量。比如在g2o或者Ceres里，你会定义一个 SE3Parameterization，它内部就是在李代数上做加法，然后指数映射回李群。
看一段伪代码，你就明白了：
// 假设我们有一个待优化的位姿 T (SE3)
// 在优化迭代中，我们计算一个增量 delta (se3)
// 然后更新 T
// 1. 计算残差和雅可比
// 2. 求解线性方程，得到 delta (在se3空间)
Eigen::Matrix<double, 6, 1> delta;
solver.solve(delta);
// 3. 将delta从se3映射回SE3
Eigen::Isometry3d delta_T = SE3::exp(delta);
// 4. 左乘更新
T = T * delta_T; // 或者 T = delta_T * T，取决于你的约定
我的经验： 在Ceres里做标定优化时，我强烈建议使用 Sophus 库或者 ceres::AutoDiffLocalParameterization。它们帮你处理好了李群李代数的求导和更新。你只需要定义好残差，剩下的交给自动求导。我曾经手写雅可比，写错一个符号，调了整整两天。从那以后，能用自动求导绝不用解析求导。
1.4 本章小结：地基打牢了，楼才盖得高
好了，这一章的内容就这些。我们回顾了：
- 坐标系是标定的「语言」，必须统一。
- 刚体变换由旋转和平移组成，旋转表示要选对。
- 李群李代数是解决流形优化问题的利器，让旋转的求导和更新变得简单。
这些东西，看起来是数学，其实是工具。你不需要成为数学家，但你需要知道怎么用这些工具去解决实际问题。下一章，我们会把这些工具真正用起来，开始搭建一个完整的标定流程。
记住，标定不是玄学，是数学和工程的结合。每一步都有据可循。