3. 坐标系与刚体运动

做SLAM，说白了就是跟坐标系打交道。你想想看，机器人身上那么多传感器——激光雷达、IMU、相机，每个都有自己的小世界。怎么把这些小世界统一起来？这就是本章要聊的事。

3.1 三个坐标系，三个世界

世界坐标系：这是全局的“上帝视角”。地图、路径规划都在这个坐标系里。我习惯把它固定在地图原点，通常是机器人启动的位置。

机器人坐标系：以机器人中心为原点，x轴朝前。你想想看，避障时判断“障碍物在我左边0.5米”就是在机器人坐标系下说的。

传感器坐标系：每个传感器都有自己的小算盘。激光雷达的扫描数据是在它自己的坐标系下测量的。嗯，这里要注意——传感器和机器人中心通常不重合，需要外参标定。

            ⚡ 核心原则：所有传感器数据最终都要变换到同一坐标系下，SLAM才能正常工作。
        

3.2 旋转表示：矩阵、四元数、欧拉角

刚体运动 = 旋转 + 平移。旋转这部分，江湖上有三种主流武功。

旋转矩阵：3x3 正交矩阵，最直观，但参数冗余（9个参数）。我在做优化时经常用它，因为求导方便。
四元数：紧凑、无万向锁。SLAM后端优化几乎都用它。我个人习惯用 Eigen 的 Quaterniond。
欧拉角：直观但容易遇到 Gimbal Lock。我曾经在调试一个无人机项目时，因为欧拉角突然跳变，差点炸机……从那以后，内部计算我再也不用欧拉角了。

表示方式	优点	缺点	SLAM常用场景
旋转矩阵	易于组合、求导	冗余、需正交约束	图优化、BA
四元数	紧凑、无奇点	不够直观	状态估计、滤波
欧拉角	人类友好	万向锁、插值困难	仅用于可视化/调试

💡 我的建议：代码里统一用四元数存储旋转，只在打印日志或调参时转成欧拉角看一眼。

3.3 Eigen 基础操作 —— 你绕不开的线性代数库

Eigen 是 C++ SLAM 的事实标准。说白了，你不会 Eigen 就没法写后端优化。下面是我几乎每天都会用到的操作。

#include <Eigen/Core>
#include <Eigen/Geometry>

// 定义三维向量
Eigen::Vector3d p(1.0, 2.0, 3.0);

// 旋转矩阵 (绕Z轴转45度)
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ());

// 四元数
Eigen::Quaterniond q(R);   // 从旋转矩阵构造
Eigen::Quaterniond q2(0.707, 0, 0, 0.707); // w,x,y,z

// 欧拉角 -> 四元数 (ZYX顺序)
Eigen::Vector3d euler(0.2, 0.1, 0.3);
Eigen::Quaterniond q_from_euler =
    Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()) *
    Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
    Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX());

// 变换矩阵 (4x4)
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(q);
T.pretranslate(Eigen::Vector3d(1,0,0));

// 应用变换
Eigen::Vector3d p_transformed = T * p;

⚠️ 我曾经踩过的坑：Eigen 四元数初始化顺序是 (w, x, y, z)，不是 (x, y, z, w)！很多新手在这里搞反，结果旋转完全不对。

3.4 坐标变换实战：从传感器到世界

假设激光雷达检测到一个点 p_lidar，怎么变成世界坐标下的点？

传感器 → 机器人：乘以外参 T_sensor_to_robot（标定得到）
机器人 → 世界：乘以 SLAM 估计的位姿 T_robot_to_world

Eigen::Vector3d p_world = T_robot_to_world * (T_sensor_to_robot * p_lidar);

你看，其实就是一连串的矩阵乘法。我在做多传感器融合时，经常要维护五六个坐标系，但只要 TF 树清晰，代码就不会乱。

            🧠 记住：坐标系变换就是左乘变换矩阵。从“源”到“目标”，链式乘下去。
        

3.5 避坑指南 & 个人经验

我曾经在标定激光雷达和相机外参时，把旋转矩阵写反了，结果点云投影到图像上全偏到天上去。后来我每次都会用单位点验证一下。
我建议所有变换都用 Isometry3d 而不是手动拼 Matrix4d，因为前者保证旋转部分正交，不会因为数值误差导致矩阵退化。
嗯，这里要注意：ROS 中 tf 的 lookupTransform 返回的 StampedTransform 可以直接转成 Eigen 的 Isometry3d，但记得先 tf::transformTFToEigen。