第4章 坐标变换与TF树
坐标变换，说白了就是让机器人知道“我在哪，传感器朝哪看”。你想想看，激光雷达扫出来的点是在雷达坐标系下，IMU的数据在imu_link，底盘里程计在odom——没有TF树，这些数据就是一堆孤岛。我做了这么多年SLAM，见过太多因为TF没配好导致定位漂移的案例。这一章，咱们就把TF2的底裤扒干净。
4.1 TF2 原理：别再搞混父子帧
TF2 本质上是一棵有向无环图，每个节点代表一个坐标系，边代表变换关系。我习惯把 base_link 当作树干，所有传感器都挂在它下面。记住一条铁律：父帧到子帧的变换，描述的是子帧在父帧下的位姿。举个例子，laser_frame 相对于 base_link 的平移和旋转，就是激光雷达在车体上的安装位置。
我在项目里遇到过一个问题：两个传感器时间戳差了几毫秒，TF2 查不到变换直接抛异常。后来我养成了习惯——发布变换时务必用 ros::Time::now() 或者带上精确时间戳，别偷懒用 0。
4.2 静态坐标变换发布
静态变换，说白了就是“一辈子不变”的关系。比如激光雷达装在机器人前方 0.3 米，仰角 5 度。这种变换只需要发布一次，TF2 会帮你缓存起来。
我个人喜欢用 tf2_ros::StaticTransformBroadcaster，代码非常简洁：
#include <tf2_ros/static_transform_broadcaster.h>
geometry_msgs::TransformStamped t;
t.header.stamp = ros::Time::now();
t.header.frame_id = "base_link";
t.child_frame_id = "laser_frame";
t.transform.translation.x = 0.3;
t.transform.translation.y = 0.0;
t.transform.translation.z = 0.15;
t.transform.rotation = tf2::toMsg(tf2::Quaternion(0, 0, 0.0436, 0.999)); // 绕Z轴5°
static tf2_ros::StaticTransformBroadcaster br;
br.sendTransform(t);static_transform_publisher 节点统一管理。这样换传感器时改一个文件就行，不用到处翻代码。
4.3 动态坐标变换发布
动态变换，典型的就是里程计 odom → base_link。每一帧里程计数据来了，都要更新这个变换。我早期做差分轮小车时，直接用轮子编码器算位姿，然后发布 TF。
看一个发布动态 TF 的典型写法：
tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "base_link";
transformStamped.transform.translation.x = odom_x;
transformStamped.transform.translation.y = odom_y;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation = tf2::toMsg(yaw_to_quat(yaw));
br.sendTransform(transformStamped);4.4 多传感器坐标系统一与可视化
多传感器融合时，最头疼的就是坐标系统一。激光雷达、摄像头、IMU 各有各的安装位置和朝向。我的做法是：
- 第一步： 用标定板或手动测量得到每个传感器相对于 base_link 的偏移量。
- 第二步： 写成静态变换，启动时一次性发布。
- 第三步： 在 Rviz 里添加 TF 显示，肉眼检查是否对齐。
你想想看，如果摄像头坐标系和激光雷达坐标系差了一个旋转，那融合出来的点云就是重影。我习惯在 Rviz 里同时显示 PointCloud2 和 Image，然后看投影是否吻合。
| 传感器 | 推荐父帧 | 变换类型 | 备注 |
|---|---|---|---|
| 2D 激光雷达 | base_link | 静态 | 通常安装在底盘前方 |
| 3D 激光雷达 | base_link | 静态 | 注意俯仰角标定 |
| IMU | base_link | 静态 | 方向对齐很重要 |
| 深度相机 | base_link | 静态 | RGB 与 depth 之间也有 TF |
| 里程计 | odom | 动态 | 由轮速或视觉里程计发布 |
Fixed Frame 设为 base_link，然后勾选 TF 插件。如果看到所有传感器帧都整齐地挂在 base_link 下，且没有红色警告，说明坐标系统一基本没问题。
嗯，这里要注意：不要手动修改 TF 树的结构。我曾经为了省事，直接把 camera 帧挂到了 laser 帧下面，结果整个树乱了套，定位模块死活找不到变换。老老实实按树形结构来，父帧统一用 base_link 或 odom。