4、坐标变换与TF树：坐标系定义（base_link, odom, map），TF广播与监听，Eigen库基础，四元数与欧拉角转换

做SLAM这些年，我最大的体会就是——坐标系搞不清楚，后面全白搭。你想想看，机器人明明在往前走，地图上却显示它在后退，这种问题十有八九是坐标变换出了岔子。今天咱们就把这块硬骨头啃下来。

4.1 三个核心坐标系：base_link、odom、map

ROS里定义了三个最重要的坐标系，我习惯把它们叫做「铁三角」。

坐标系	含义	特点
base_link	机器人本体坐标系	随机器人移动，原点一般在底盘中心
odom	里程计坐标系	以起始点为原点，存在漂移
map	全局地图坐标系	固定不变，用于建图和定位

base_link 说白了就是「机器人的身体」。激光雷达装在哪儿？IMU装在哪儿？都是以base_link为基准定义的。我在项目中遇到过有人把激光雷达的安装偏移搞反了，结果建出来的地图全是歪的——嗯，排查了两天才找到原因。

odom 是里程计坐标系。它从机器人启动时的位置开始算起。但有个问题——里程计会漂移。你让机器人走10米，odom告诉你走了10.05米，这就是误差。所以odom坐标系是局部一致、全局不一致的。

map 是全局坐标系。它一旦建立就固定不动了。SLAM的核心任务之一，就是找到base_link在map坐标系下的位姿。说白了，就是回答「我在哪儿」这个问题。

4.2 TF广播与监听

TF（Transform）是ROS里管理坐标变换的利器。我刚开始学的时候觉得它很玄乎，其实说白了就是发布坐标系之间的相对关系。

4.2.1 广播TF

假设你的机器人有个激光雷达，安装在前方0.3米处。你需要告诉系统：base_link和laser_frame之间的关系。

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "robot_tf_publisher");
    ros::NodeHandle n;
    tf::TransformBroadcaster broadcaster;
    
    ros::Rate rate(10.0);
    while (n.ok()) {
        // 激光雷达安装在base_link前方0.3米
        tf::Transform transform;
        transform.setOrigin(tf::Vector3(0.3, 0.0, 0.1));
        tf::Quaternion q;
        q.setRPY(0, 0, 0);
        transform.setRotation(q);
        
        // 发布变换：base_link → laser_frame
        broadcaster.sendTransform(
            tf::StampedTransform(transform, ros::Time::now(), 
                                 "base_link", "laser_frame"));
        rate.sleep();
    }
    return 0;
}

这里有个坑——时间戳一定要用最新的。我曾经犯过一个错误，用了ros::Time(0)，结果TF树一直报「extrapolation into the future」的警告。折腾了半天才发现是时间戳的问题。

4.2.2 监听TF

监听TF更常用。比如你要把激光雷达的数据转换到base_link坐标系下：

#include <ros/ros.h>
#include <tf/transform_listener.h>

tf::TransformListener listener;
tf::StampedTransform transform;

try {
    // 等待变换可用
    listener.waitForTransform("base_link", "laser_frame", 
                              ros::Time(0), ros::Duration(3.0));
    // 获取变换
    listener.lookupTransform("base_link", "laser_frame", 
                             ros::Time(0), transform);
} catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
}

4.3 Eigen库基础

Eigen是C++里做矩阵运算的标配库。做SLAM绕不开它。你想想看，坐标变换本质上就是矩阵乘法。

4.3.1 基本用法

#include <Eigen/Dense>

// 定义向量
Eigen::Vector3d v(1.0, 2.0, 3.0);

// 定义矩阵
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d::UnitZ()) *
    Eigen::AngleAxisd(0.5*M_PI,  Eigen::Vector3d::UnitY()) *
    Eigen::AngleAxisd(0.33*M_PI, Eigen::Vector3d::UnitX());

// 矩阵乘法
Eigen::Vector3d v_rotated = R * v;

// 齐次变换矩阵
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = Eigen::Vector3d(1.0, 2.0, 3.0);

我个人习惯用 Eigen::Isometry3d 来表示变换矩阵，它比手动拼 Matrix4d 更安全：

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.pretranslate(Eigen::Vector3d(1.0, 2.0, 3.0));

4.4 四元数与欧拉角转换

说到这个我就来气。欧拉角有万向锁问题，但很多人就是喜欢用它。我建议你内部计算一律用四元数，只在显示和调试时转成欧拉角。

4.4.1 为什么用四元数？

• 无万向锁：欧拉角在俯仰角±90°时会丢失一个自由度
• 插值平滑：四元数的球面插值（Slerp）比欧拉角线性插值自然得多
• 计算稳定：四元数乘法比矩阵乘法更高效

4.4.2 转换示例

#include <Eigen/Geometry>

// 欧拉角 → 四元数
double roll = 0.1, pitch = 0.2, yaw = 0.3;
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX()) *
    Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
    Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ());

// 四元数 → 欧拉角
Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
// 注意：返回顺序是 yaw, pitch, roll

4.4.3 实战中的小技巧

在SLAM里，我经常需要把四元数转成旋转矩阵：

// 四元数 → 旋转矩阵
Eigen::Matrix3d R = q.toRotationMatrix();

// 旋转矩阵 → 四元数
Eigen::Quaterniond q2(R);

// 检查四元数是否合法
if (q2.norm() < 1e-6) {
    ROS_WARN("四元数接近零，可能有问题！");
}

嗯，这里要注意——四元数一定要归一化。数值误差会导致四元数的模不再是1，这时候用它做旋转会出问题。我习惯每次用之前都调一下 q.normalize()。

4.5 总结

坐标变换是SLAM的基石。说白了，整个SLAM系统就是在不断回答三个问题：

1. 我在哪儿？（base_link在map中的位姿）
2. 我从哪儿来？（odom到base_link的变换）
3. 周围长啥样？（激光数据转换到全局坐标系）

搞清楚了这三个坐标系的关系，你的SLAM系统就成功了一半。剩下的，就是调参和debug了——那又是另一个故事了。