第五部分：Transform模块详解
免责声明
本文档仅供学习和技术研究使用,内容基于 Apollo 开源项目的公开资料和代码分析整理而成。文档中的技术实现细节、架构设计等内容可能随 Apollo 项目更新而变化。使用本文档所述技术方案时,请以 Apollo 官方最新文档为准。
声明事项:
- 本文档不构成任何商业使用建议
- 涉及自动驾驶技术的应用需遵守当地法律法规
- 作者不对因使用本文档内容产生的任何后果承担责任
- Apollo 为百度公司注册商标,本文档为非官方技术解析
1. Transform模块概述
1.1 模块定位与作用
Transform模块是Apollo自动驾驶系统中负责坐标系变换管理的基础设施模块,它为所有模块提供统一的坐标转换服务。
核心功能:
- 管理多坐标系之间的变换关系
- 提供坐标系查询和转换接口
- 维护坐标变换历史记录
- 发布静态坐标变换(传感器外参)
- 支持动态坐标变换(车辆位姿)
与其他模块的关系:
┌─────────────────────────────────────────────────────────────────┐
│ Transform Module │
│ (坐标系变换中心 - 单例Buffer) │
│ ┌──────────────────────────────────────────────────────────┐ │
│ │ Buffer (TF Tree) │ │
│ │ - world │ │
│ │ └─ localization │ │
│ │ ├─ novatel (GNSS/INS) │ │
│ │ │ └─ velodyne64 (激光雷达) │ │
│ │ │ ├─ front_6mm (相机) │ │
│ │ │ └─ radar_front (毫米波雷达) │ │
│ │ └─ imu │ │
│ └──────────────────────────────────────────────────────────┘ │
└─────────────────────────┬───────────────────────────────────────┘
│ lookupTransform()
┌─────────────┼─────────────┬──────────────┐
↓ ↓ ↓ ↓
┌─────────┐ ┌──────────┐ ┌─────────┐ ┌──────────┐
│Perception│ │Planning │ │Control │ │Localization│
│(传感器 │ │(路径规划)│ │(控制) │ │(定位) │
│ 数据融合)│ └──────────┘ └─────────┘ └──────────┘
└─────────┘1.2 技术特点
- 基于TF2库: 继承ROS TF2库的成熟实现,保证稳定性和兼容性
- 单例模式: Buffer采用单例模式,全局唯一的坐标系树
- 历史缓存: 默认缓存10秒的坐标变换历史,支持时间旅行查询
- QoS优化: 静态变换使用TRANSIENT_LOCAL保证新订阅者能获取
- 自动时间补偿: 支持仿真模式和真实模式的时间处理
- 线程安全: 多线程环境下的并发访问保护
1.3 关键概念
1.3.1 坐标系(Frame)
坐标系是三维空间中的参考框架,由原点和三个正交轴(X, Y, Z)定义。
Apollo中的主要坐标系:
| 坐标系名称 | 类型 | 说明 | 原点位置 |
|---|---|---|---|
| world | 静态 | 世界坐标系(UTM) | 地球表面某一点 |
| localization | 动态 | 定位坐标系 | 车辆初始位置 |
| novatel | 静态/动态 | GNSS/INS坐标系 | GNSS天线 |
| imu | 静态 | IMU坐标系 | IMU传感器中心 |
| velodyne64 | 静态 | 激光雷达坐标系 | 激光雷达中心 |
| front_6mm | 静态 | 相机坐标系 | 相机光学中心 |
| radar_front | 静态 | 毫米波雷达坐标系 | 雷达中心 |
1.3.2 坐标变换(Transform)
坐标变换描述两个坐标系之间的空间关系,包括:
-
平移(Translation): 原点位置差(x, y, z)
-
旋转(Rotation): 姿态差异(四元数 qx, qy, qz, qw)
Frame A Frame B
Z↑ Z↑
| |
| Y | Y
| ↗ | ↗
|/ |/
O────→X O────→XTransform A->B:
translation: (tx, ty, tz)
rotation: (qx, qy, qz, qw)
1.3.3 TF树(Transform Tree)
TF树是有向无环图(DAG),表示所有坐标系之间的连接关系。
示例TF树:
world
│
↓
localization
╱ ╲
↓ ↓
novatel imu
│
↓
velodyne64
╱ ╲
↓ ↓
front_6mm radar_front
特点:
- 每个frame只有一个父frame
- 从任意frame到另一个frame的路径唯一
- 支持双向查询(A->B和B->A)1.4 数据流
[静态变换发布流程]
StaticTransformComponent
↓ (启动时加载外参文件)
解析YAML文件
↓ (构造TransformStamped)
发布到 /tf_static channel
↓ (QoS: TRANSIENT_LOCAL)
Buffer接收并缓存
↓
其他模块查询使用
[动态变换发布流程]
Localization模块
↓ (实时计算车辆位姿)
TransformBroadcaster
↓ (构造world->localization变换)
发布到 /tf channel
↓ (周期性更新,100Hz)
Buffer接收并缓存历史
↓
Planning/Control/Perception查询
[坐标变换查询流程]
Perception模块
↓ (需要将激光点云转到车体坐标系)
Buffer::lookupTransform("localization", "velodyne64", time)
↓ (TF树路径搜索)
沿路径累积变换: localization <- novatel <- velodyne64
↓ (矩阵乘法)
返回变换结果
↓
应用变换到点云数据2. 坐标系体系
2.1 坐标系层次结构
Apollo的坐标系采用层次化设计,从全局到局部依次为:
Level 1: 全局坐标系
┌────────────────────────────────────────┐
│ world (UTM坐标系) │
│ - 东北天坐标系(ENU: East-North-Up) │
│ - 覆盖整个地图区域 │
│ - 原点为地图区域的某一参考点 │
└────────────┬───────────────────────────┘
│ (动态变换,来自Localization)
↓
Level 2: 车辆定位坐标系
┌────────────────────────────────────────┐
│ localization │
│ - 车辆在world中的当前位置和姿态 │
│ - 原点为车辆初始位置 │
│ - 动态更新(100Hz) │
└────────────┬───────────────────────────┘
│ (静态变换,传感器外参)
┌──────┴──────┐
↓ ↓
Level 3: 主传感器坐标系
┌─────────────┐ ┌────────────┐
│ novatel │ │ imu │
│ (GNSS/INS) │ │ (IMU) │
└──────┬──────┘ └────────────┘
│ (静态变换,传感器外参)
↓
Level 4: 感知传感器坐标系
┌──────────────┬─────────────┬──────────────┐
│ velodyne64 │ front_6mm │ radar_front │
│ (激光雷达) │ (相机) │ (毫米波雷达) │
└──────────────┴─────────────┴──────────────┘2.2 坐标系定义规范
2.2.1 右手坐标系规则
Apollo统一使用右手坐标系:
-
X轴: 前进方向(车辆前方)
-
Y轴: 左侧方向(车辆左侧)
-
Z轴: 向上方向(天空方向)
车辆坐标系示例:
Z(Up)
↑
|
|
O────→ X(Forward)
╱
╱
Y(Left)右手法则: 右手四指从X轴握向Y轴,大拇指指向Z轴
2.2.2 主要坐标系详细定义
1. world坐标系 (UTM ENU)
yaml
名称: world
类型: 静态全局坐标系
定义:
X轴: 东向(East)
Y轴: 北向(North)
Z轴: 天向(Up)
原点: 地图区域参考点(通常为地图西南角)
用途:
- 高精地图坐标
- 全局路径规划
- 长距离导航2. localization坐标系
yaml
名称: localization
类型: 动态车辆坐标系
定义:
X轴: 车辆前进方向
Y轴: 车辆左侧方向
Z轴: 车辆上方向
原点: 车辆后轴中心投影到地面
更新频率: 100Hz
与world关系: 动态变换(6DOF: x,y,z,roll,pitch,yaw)
用途:
- 局部轨迹规划
- 车辆控制
- 障碍物表示3. novatel坐标系
yaml
名称: novatel
类型: 静态传感器坐标系
定义:
X轴: GNSS天线前向
Y轴: GNSS天线左向
Z轴: GNSS天线上向
原点: GNSS天线相位中心
与localization关系: 静态外参(通过标定获得)
典型外参:
translation: (0.0, 0.5, 1.2) # 米
rotation: (0, 0, 0, 1) # 四元数
用途:
- GNSS数据融合
- INS姿态表示4. velodyne64坐标系
yaml
名称: velodyne64
类型: 静态传感器坐标系
定义:
X轴: 激光雷达前向
Y轴: 激光雷达左向
Z轴: 激光雷达上向
原点: 激光雷达旋转中心
与novatel关系: 静态外参
典型外参:
translation: (0.0, 0.0, -0.3) # 相对novatel
rotation: (0, 0, 0, 1)
用途:
- 点云数据处理
- 障碍物检测
- 地图匹配5. camera坐标系 (front_6mm)
yaml
名称: front_6mm
类型: 静态传感器坐标系
定义:
X轴: 相机右向
Y轴: 相机下向
Z轴: 相机前向(光轴方向)
原点: 相机光学中心
与velodyne64关系: 静态外参
典型外参:
translation: (0.3, -0.2, -0.1)
rotation: (0.5, -0.5, 0.5, -0.5) # 90度旋转
注意: 相机坐标系与车辆坐标系的轴定义不同!
用途:
- 图像目标检测
- 车道线检测
- 红绿灯识别2.3 坐标变换链
2.3.1 典型变换链示例
示例1: 将激光点云转换到车体坐标系
目标: 将velodyne64坐标系下的点云转到localization坐标系
变换链:
velodyne64 -> novatel -> localization
查询代码:
auto tf = buffer->lookupTransform(
"localization", // target_frame
"velodyne64", // source_frame
cyber::Time::Now()
);
变换计算:
T_loc_velo = T_loc_nova * T_nova_velo
其中:
- T_nova_velo: 静态外参(标定获得)
- T_loc_nova: localization -> novatel的逆变换示例2: 将相机检测结果投影到世界坐标系
目标: 将front_6mm坐标系下的检测框转到world坐标系
变换链:
front_6mm -> velodyne64 -> novatel -> localization -> world
查询代码:
auto tf = buffer->lookupTransform(
"world", // target_frame
"front_6mm", // source_frame
cyber::Time(detection_timestamp)
);
变换计算:
T_world_cam = T_world_loc * T_loc_nova * T_nova_velo * T_velo_cam2.3.2 时间同步问题
问题: 传感器数据采集时刻与查询时刻可能不同。
解决方案: 使用带时间戳的变换查询。
cpp
// 错误方式: 使用当前时间
auto tf_now = buffer->lookupTransform(
"localization", "velodyne64",
cyber::Time::Now() // 错误!
);
// 正确方式: 使用数据时间戳
auto tf_correct = buffer->lookupTransform(
"localization", "velodyne64",
cyber::Time(pointcloud->header().timestamp_sec()) // 正确!
);时间插值:
Buffer内部维护变换历史,支持时间插值:
Buffer历史记录:
t=1.0s: T_loc_nova = [x=10.0, y=5.0, ...]
t=1.1s: T_loc_nova = [x=10.5, y=5.2, ...]
t=1.2s: T_loc_nova = [x=11.0, y=5.4, ...]
查询 t=1.05s:
结果 = 线性插值(T@1.0s, T@1.1s, ratio=0.5)
= [x=10.25, y=5.1, ...]2.4 外参标定
2.4.1 外参定义
外参(Extrinsic Parameters)描述传感器之间的相对位置和姿态关系。
标定目标:
- novatel -> velodyne64
- velodyne64 -> cameras
- velodyne64 -> radars
2.4.2 标定流程
1. 物理测量法(粗标定)
步骤:
1. 测量传感器之间的物理距离
2. 使用水平仪测量角度偏差
3. 记录到YAML文件
优点: 快速
缺点: 精度低(cm级,度级)2. 数据标定法(精标定)
激光雷达-相机标定:
1. 采集标定板数据(Checkerboard)
2. 检测标定板在图像和点云中的位置
3. 最小化重投影误差
4. 优化外参矩阵
工具: Apollo sensor_calibration工具
精度: mm级, 0.1度级2.4.3 外参表示
变换矩阵表示 (4x4齐次矩阵)
T = [R | t]
[0 | 1]
其中:
R: 3x3旋转矩阵
t: 3x1平移向量
示例:
T_nova_velo = [0.999 -0.001 0.000 | 0.00]
[0.001 0.999 0.000 | 0.00]
[0.000 0.000 1.000 |-0.30]
[0.000 0.000 0.000 | 1.00]四元数+平移表示 (Apollo采用)
yaml
translation:
x: 0.0
y: 0.0
z: -0.3
rotation: # 四元数 (qx, qy, qz, qw)
x: 0.0
y: 0.0
z: 0.0
w: 1.0
转换关系:
旋转矩阵 R <-> 四元数 q
Apollo使用四元数表示,因为:
- 存储空间小(4个数 vs 9个数)
- 插值容易(SLERP)
- 避免万向锁问题3. 核心架构设计
3.1 整体架构
┌──────────────────────────────────────────────────────────────────┐
│ Transform Module │
│ │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ StaticTransformComponent (CyberRT Component) │ │
│ │ - 加载外参配置文件 │ │
│ │ - 启动时一次性发布静态变换 │ │
│ │ - Channel: /tf_static (QoS: TRANSIENT_LOCAL) │ │
│ └────────────────┬───────────────────────────────────────────┘ │
│ │ │
│ │ TransformStampeds │
│ ↓ │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ Buffer (Singleton, extends tf2::BufferCore) │ │
│ │ ┌──────────────────────────────────────────────────────┐ │ │
│ │ │ TF2 BufferCore │ │ │
│ │ │ - 坐标变换树管理 │ │ │
│ │ │ - 历史缓存(默认10秒) │ │ │
│ │ │ - 路径搜索算法 │ │ │
│ │ │ - 时间插值 │ │ │
│ │ └──────────────────────────────────────────────────────┘ │ │
│ │ │ │
│ │ ┌──────────────────────────────────────────────────────┐ │ │
│ │ │ CyberRT Integration │ │ │
│ │ │ - Reader: /tf, /tf_static │ │ │
│ │ │ - 消息格式转换(Protobuf <-> TF2) │ │ │
│ │ │ - 时间跳变检测 │ │ │
│ │ └──────────────────────────────────────────────────────┘ │ │
│ └────────────────┬───────────────────────────────────────────┘ │
│ │ │
│ │ lookupTransform() / canTransform() │
│ ↓ │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ TransformBroadcaster (Helper Class) │ │
│ │ - 简化Transform发布 │ │
│ │ - Writer: /tf │ │
│ └────────────────────────────────────────────────────────────┘ │
└──────────────────────────────────────────────────────────────────┘3.2 类层次结构
BufferInterface (抽象接口)
│
├─ virtual lookupTransform()
├─ virtual canTransform()
└─ template transform<T>()
↑
│ (继承)
│
Buffer
│
├─ (继承) tf2::BufferCore (TF2库核心)
│ │
│ ├─ setTransform()
│ ├─ lookupTransform()
│ ├─ canTransform()
│ └─ allFramesAsYAML()
│
├─ (组合) CyberRT Node
│ │
│ ├─ Reader<TransformStampeds> (订阅/tf)
│ └─ Reader<TransformStampeds> (订阅/tf_static)
│
└─ (单例) DECLARE_SINGLETON(Buffer)
TransformBroadcaster
│
├─ (组合) CyberRT Node
│ │
│ └─ Writer<TransformStampeds> (发布到/tf)
│
├─ SendTransform(single)
└─ SendTransform(vector)
StaticTransformComponent
│
├─ (继承) cyber::Component<>
│
├─ (组合) Writer<TransformStampeds> (发布到/tf_static)
│
├─ ParseFromYaml()
└─ SendTransforms()3.3 数据结构
3.3.1 Protobuf消息定义
transform.proto
protobuf
// modules/common_msgs/transform_msgs/transform.proto
syntax = "proto2";
package apollo.transform;
import "modules/common_msgs/basic_msgs/geometry.proto";
import "modules/common_msgs/basic_msgs/header.proto";
// 3D变换(平移+旋转)
message Transform {
optional apollo.common.Point3D translation = 1; // (x, y, z)
optional apollo.common.Quaternion rotation = 2; // (qx, qy, qz, qw)
}
// 带时间戳的变换
message TransformStamped {
optional apollo.common.Header header = 1; // 时间戳+frame_id
optional string child_frame_id = 2; // 子坐标系名称
optional Transform transform = 3; // 变换数据
}
// 变换集合(批量发布)
message TransformStampeds {
optional apollo.common.Header header = 1;
repeated TransformStamped transforms = 2;
}static_transform_conf.proto
protobuf
// modules/transform/proto/static_transform_conf.proto
syntax = "proto2";
package apollo.static_transform;
// 外参文件配置
message ExtrinsicFile {
optional string frame_id = 1; // 父坐标系
optional string child_frame_id = 2; // 子坐标系
optional string file_path = 3; // YAML文件路径
optional bool enable = 4; // 是否启用
}
// 静态变换配置
message Conf {
repeated ExtrinsicFile extrinsic_file = 1;
}3.3.2 TF2内部数据结构
cpp
// tf2库中的核心数据结构
namespace geometry_msgs {
struct TransformStamped {
struct {
uint64_t stamp; // 纳秒时间戳
std::string frame_id; // 父坐标系
uint32_t seq; // 序列号
} header;
std::string child_frame_id; // 子坐标系
struct {
struct { double x, y, z; } translation;
struct { double x, y, z, w; } rotation; // 四元数
} transform;
};
}
// BufferCore内部存储
namespace tf2 {
class BufferCore {
private:
// 坐标系ID -> TransformStorage映射
std::map<CompactFrameID, TimeCache> frames_;
// TimeCache: 时间戳 -> Transform
class TimeCache {
std::list<TransformStorage> storage_; // 按时间排序
uint64_t max_storage_time_; // 最大缓存时间(默认10s)
};
};
}3.4 关键流程
3.4.1 初始化流程
[Transform模块启动]
↓
1. StaticTransformComponent::Init()
↓
加载配置文件: static_transform_conf.pb.txt
↓
遍历所有extrinsic_file
↓
ParseFromYaml(): 解析YAML外参文件
↓
构造TransformStampeds消息
↓
发布到 /tf_static (QoS: TRANSIENT_LOCAL, Depth=1)
↓
2. Buffer单例自动初始化
↓
创建CyberRT Node
↓
创建Reader订阅 /tf (动态变换)
↓
创建Reader订阅 /tf_static (静态变换)
↓
注册回调: SubscriptionCallbackImpl()
↓
[等待变换数据]3.4.2 变换发布流程
[动态变换发布 - 以Localization为例]
Localization模块计算车辆位姿
↓
构造TransformStamped:
header.frame_id = "world"
child_frame_id = "localization"
transform.translation = (x, y, z)
transform.rotation = (qx, qy, qz, qw)
header.timestamp_sec = current_time
↓
TransformBroadcaster::SendTransform()
↓
发布到 /tf channel
↓
Buffer::SubscriptionCallbackImpl()
↓
转换为TF2格式: geometry_msgs::TransformStamped
↓
tf2::BufferCore::setTransform()
↓
存储到时间缓存: frames_["localization"].insert(tf, time)
↓
[变换可供查询]3.4.3 变换查询流程
[模块查询变换 - 以Perception为例]
Perception需要将点云从velodyne64转到localization
↓
Buffer::lookupTransform("localization", "velodyne64", time)
↓
1. 检查两个frame是否存在
↓
2. 搜索TF树路径: localization <- novatel <- velodyne64
↓
3. 对每条边:
- 在TimeCache中查找time对应的变换
- 如果时间不精确匹配,进行线性插值
↓
4. 沿路径累积变换(矩阵乘法):
T_loc_velo = T_loc_nova * T_nova_velo
↓
5. 转换为Protobuf格式返回
↓
Perception应用变换到点云4. Buffer类详解
4.1 Buffer类定义
文件位置: modules/transform/buffer.h
cpp
namespace apollo {
namespace transform {
// 继承BufferInterface接口和tf2::BufferCore核心
class Buffer : public BufferInterface, public tf2::BufferCore {
public:
// 初始化: 创建Node和订阅者
int Init();
// 查询变换(简单版本)
virtual TransformStamped lookupTransform(
const std::string& target_frame, // 目标坐标系
const std::string& source_frame, // 源坐标系
const cyber::Time& time, // 查询时间
const float timeout_second = 0.01f) // 超时时间
const;
// 查询变换(高级版本,带fixed_frame)
virtual TransformStamped lookupTransform(
const std::string& target_frame,
const cyber::Time& target_time,
const std::string& source_frame,
const cyber::Time& source_time,
const std::string& fixed_frame, // 固定坐标系(用于时间插值)
const float timeout_second = 0.01f)
const;
// 检查变换是否可用
virtual bool canTransform(
const std::string& target_frame,
const std::string& source_frame,
const cyber::Time& target_time,
const float timeout_second = 0.01f,
std::string* errstr = nullptr) // 错误信息输出
const;
// 获取最新静态变换
bool GetLatestStaticTF(
const std::string& frame_id,
const std::string& child_frame_id,
TransformStamped* tf);
private:
// 动态变换回调
void SubscriptionCallback(
const std::shared_ptr<const TransformStampeds>& transform);
// 静态变换回调
void StaticSubscriptionCallback(
const std::shared_ptr<const TransformStampeds>& transform);
// 通用回调实现
void SubscriptionCallbackImpl(
const std::shared_ptr<const TransformStampeds>& transform,
bool is_static);
// TF2格式转Protobuf格式
void TF2MsgToCyber(
const geometry_msgs::TransformStamped& tf2_trans_stamped,
TransformStamped& trans_stamped) const;
private:
std::unique_ptr<cyber::Node> node_; // CyberRT节点
std::shared_ptr<cyber::Reader<TransformStampeds>> message_subscriber_tf_; // 动态订阅者
std::shared_ptr<cyber::Reader<TransformStampeds>> message_subscriber_tf_static_; // 静态订阅者
cyber::Time last_update_; // 最后更新时间
std::vector<geometry_msgs::TransformStamped> static_msgs_; // 静态变换缓存
DECLARE_SINGLETON(Buffer) // 单例声明
};
} // namespace transform
} // namespace apollo4.2 初始化实现
cpp
// buffer.cc:38-59
int Buffer::Init() {
// 创建唯一的Node名称(避免冲突)
const std::string node_name =
absl::StrCat("transform_listener_", Time::Now().ToNanosecond());
node_ = cyber::CreateNode(node_name);
// 订阅动态变换 /tf
apollo::cyber::proto::RoleAttributes attr;
attr.set_channel_name("/tf");
message_subscriber_tf_ = node_->CreateReader<TransformStampeds>(
attr, [&](const std::shared_ptr<const TransformStampeds>& msg_evt) {
SubscriptionCallbackImpl(msg_evt, false); // is_static=false
});
// 订阅静态变换 /tf_static (QoS: TF_STATIC profile)
apollo::cyber::proto::RoleAttributes attr_static;
attr_static.set_channel_name(FLAGS_tf_static_topic); // "/tf_static"
attr_static.mutable_qos_profile()->CopyFrom(
apollo::cyber::transport::QosProfileConf::QOS_PROFILE_TF_STATIC);
message_subscriber_tf_static_ = node_->CreateReader<TransformStampeds>(
attr_static, [&](const std::shared_ptr<TransformStampeds>& msg_evt) {
SubscriptionCallbackImpl(msg_evt, true); // is_static=true
});
return cyber::SUCC;
}QoS配置说明:
cpp
// QOS_PROFILE_TF_STATIC配置:
{
history: KEEP_LAST,
depth: 1, // 只保留最新一条
reliability: RELIABLE, // 可靠传输
durability: TRANSIENT_LOCAL, // 新订阅者能获取历史消息
}
作用:
- 确保晚启动的模块也能获取静态变换
- 避免因模块启动顺序导致的变换缺失4.3 变换接收与存储
cpp
// buffer.cc:71-121
void Buffer::SubscriptionCallbackImpl(
const std::shared_ptr<const TransformStampeds>& msg_evt,
bool is_static) {
cyber::Time now = Clock::Now();
std::string authority = "cyber_tf";
// 检测时间跳变(系统时间回退)
if (now.ToNanosecond() < last_update_.ToNanosecond()) {
AINFO << "Detected jump back in time. Clearing TF buffer.";
clear(); // 清空所有缓存
// 重新加载静态变换
for (auto& msg : static_msgs_) {
setTransform(msg, authority, true);
}
}
last_update_ = now;
// 处理每个变换
for (int i = 0; i < msg_evt->transforms_size(); i++) {
try {
geometry_msgs::TransformStamped trans_stamped;
// 解析header
const auto& header = msg_evt->transforms(i).header();
trans_stamped.header.stamp =
static_cast<uint64_t>(header.timestamp_sec() * 1e9); // 转纳秒
trans_stamped.header.frame_id = header.frame_id();
trans_stamped.header.seq = header.sequence_num();
// 解析child_frame_id
trans_stamped.child_frame_id = msg_evt->transforms(i).child_frame_id();
// 解析平移
const auto& transform = msg_evt->transforms(i).transform();
trans_stamped.transform.translation.x = transform.translation().x();
trans_stamped.transform.translation.y = transform.translation().y();
trans_stamped.transform.translation.z = transform.translation().z();
// 解析旋转(四元数)
trans_stamped.transform.rotation.x = transform.rotation().qx();
trans_stamped.transform.rotation.y = transform.rotation().qy();
trans_stamped.transform.rotation.z = transform.rotation().qz();
trans_stamped.transform.rotation.w = transform.rotation().qw();
// 如果是静态变换,缓存到static_msgs_
if (is_static) {
static_msgs_.push_back(trans_stamped);
}
// 存储到TF2 BufferCore
setTransform(trans_stamped, authority, is_static);
} catch (tf2::TransformException& ex) {
AERROR << "Failure to set received transform:" << ex.what();
}
}
}时间跳变处理:
场景: 仿真模式下,系统时间可能回退
问题: 如果不处理,TF树中会出现"未来"的变换,导致查询失败
解决方案:
1. 检测到时间回退时清空整个TF树
2. 重新加载静态变换(从static_msgs_缓存)
3. 等待新的动态变换到来4.4 变换查询实现
4.4.1 简单查询
cpp
// buffer.cc:168-178
TransformStamped Buffer::lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
const cyber::Time& time,
const float timeout_second) const {
// 转换为TF2时间格式
tf2::Time tf2_time(time.ToNanosecond());
// 调用TF2核心库查询
geometry_msgs::TransformStamped tf2_trans_stamped =
tf2::BufferCore::lookupTransform(target_frame, source_frame, tf2_time);
// 转换为Protobuf格式
TransformStamped trans_stamped;
TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
return trans_stamped;
}4.4.2 高级查询(带fixed_frame)
cpp
// buffer.cc:180-193
TransformStamped Buffer::lookupTransform(
const std::string& target_frame,
const cyber::Time& target_time,
const std::string& source_frame,
const cyber::Time& source_time,
const std::string& fixed_frame,
const float timeout_second) const {
// 调用TF2核心库高级查询
geometry_msgs::TransformStamped tf2_trans_stamped =
tf2::BufferCore::lookupTransform(
target_frame, target_time.ToNanosecond(),
source_frame, source_time.ToNanosecond(),
fixed_frame);
TransformStamped trans_stamped;
TF2MsgToCyber(tf2_trans_stamped, trans_stamped);
return trans_stamped;
}fixed_frame的作用:
场景: 查询运动物体在不同时刻的相对位置
例子: 计算t1时刻车辆位置到t2时刻车辆位置的变换
target_frame: "localization"
target_time: t2
source_frame: "localization"
source_time: t1
fixed_frame: "world" (固定参考系)
计算:
T = T_world_loc(t2)^-1 * T_world_loc(t1)
= 从t1位置到t2位置的变换4.4.3 变换可用性检查
cpp
// buffer.cc:195-220 (简化版)
bool Buffer::canTransform(
const std::string& target_frame,
const std::string& source_frame,
const cyber::Time& time,
const float timeout_second,
std::string* errstr) const {
uint64_t timeout_ns = static_cast<uint64_t>(timeout_second * 1e9);
uint64_t start_time = Clock::Now().ToNanosecond();
// 轮询查询(直到超时)
while (Clock::Now().ToNanosecond() < start_time + timeout_ns &&
!cyber::IsShutdown()) {
errstr->clear();
bool retval = tf2::BufferCore::canTransform(
target_frame, source_frame, time.ToNanosecond(), errstr);
if (retval) {
return true; // 变换可用
} else {
if (!cyber::common::GlobalData::Instance()->IsRealityMode()) {
break; // 仿真模式不等待
}
const int sleep_time_ms = 3;
AWARN << "BufferCore::canTransform failed: " << *errstr;
std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time_ms));
}
}
*errstr = *errstr + ":timeout";
return false;
}使用模式:
cpp
// 模式1: 立即检查(不等待)
std::string error;
if (buffer->canTransform("localization", "velodyne64",
cyber::Time::Now(), 0.0f, &error)) {
// 变换可用
auto tf = buffer->lookupTransform(...);
} else {
AERROR << "Transform not available: " << error;
}
// 模式2: 等待变换可用(最多100ms)
if (buffer->canTransform("localization", "velodyne64",
cyber::Time::Now(), 0.1f, &error)) {
// 变换可用(等待期间可能收到新的变换消息)
auto tf = buffer->lookupTransform(...);
}4.5 使用示例
示例1: 点云坐标转换
cpp
#include "modules/transform/buffer.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
// 获取Buffer单例
auto* buffer = apollo::transform::Buffer::Instance();
// 接收点云数据
void OnPointCloud(const std::shared_ptr<PointCloud>& cloud) {
// 检查变换是否可用
std::string error;
cyber::Time cloud_time(cloud->header().timestamp_sec());
if (!buffer->canTransform("localization", "velodyne64",
cloud_time, 0.01f, &error)) {
AERROR << "Transform not available: " << error;
return;
}
// 查询变换
TransformStamped tf = buffer->lookupTransform(
"localization", // 目标坐标系
"velodyne64", // 源坐标系
cloud_time // 点云时间戳
);
// 应用变换到每个点
for (auto& point : cloud->points()) {
Eigen::Vector3d p_velo(point.x(), point.y(), point.z());
Eigen::Vector3d p_loc = TransformPoint(tf, p_velo);
point.set_x(p_loc.x());
point.set_y(p_loc.y());
point.set_z(p_loc.z());
}
}
// 辅助函数: 应用变换到点
Eigen::Vector3d TransformPoint(const TransformStamped& tf,
const Eigen::Vector3d& point) {
// 提取平移
Eigen::Vector3d t(tf.transform().translation().x(),
tf.transform().translation().y(),
tf.transform().translation().z());
// 提取旋转(四元数)
Eigen::Quaterniond q(tf.transform().rotation().qw(),
tf.transform().rotation().qx(),
tf.transform().rotation().qy(),
tf.transform().rotation().qz());
// 应用变换: p' = R*p + t
return q * point + t;
}示例2: 模板接口使用
cpp
#include "modules/transform/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
auto* buffer = apollo::transform::Buffer::Instance();
// 转换Point3D
apollo::common::Point3D point_in_camera;
point_in_camera.set_x(10.0);
point_in_camera.set_y(5.0);
point_in_camera.set_z(2.0);
// 使用模板接口自动转换
apollo::common::Point3D point_in_world = buffer->transform(
point_in_camera,
"world", // 目标坐标系
cyber::Time::Now()
);
AINFO << "Point in world: ("
<< point_in_world.x() << ", "
<< point_in_world.y() << ", "
<< point_in_world.z() << ")";5. TransformBroadcaster详解
5.1 类定义
文件位置: modules/transform/transform_broadcaster.h
cpp
namespace apollo {
namespace transform {
/**
* @brief 简化Transform发布的工具类
*
* 负责将TransformStamped消息发布到/tf channel,
* 供Buffer接收和其他模块查询使用。
*/
class TransformBroadcaster {
public:
/**
* @brief 构造函数
* @param node CyberRT节点(用于创建Writer)
*/
explicit TransformBroadcaster(const std::shared_ptr<cyber::Node>& node);
/**
* @brief 发布单个变换
* @param transform 要发布的变换
*/
void SendTransform(const TransformStamped& transform);
/**
* @brief 批量发布变换
* @param transforms 变换向量
*/
void SendTransform(const std::vector<TransformStamped>& transforms);
private:
std::shared_ptr<cyber::Node> node_; // CyberRT节点
std::shared_ptr<cyber::Writer<TransformStampeds>> writer_; // 消息发布器
};
} // namespace transform
} // namespace apollo5.2 实现
cpp
// transform_broadcaster.cc:24-43
TransformBroadcaster::TransformBroadcaster(
const std::shared_ptr<cyber::Node>& node)
: node_(node) {
// 创建Writer,发布到/tf channel
cyber::proto::RoleAttributes attr;
attr.set_channel_name(FLAGS_tf_topic); // "/tf"
writer_ = node_->CreateWriter<TransformStampeds>(attr);
}
// 发布单个变换(转换为向量后调用批量发布)
void TransformBroadcaster::SendTransform(const TransformStamped& transform) {
std::vector<TransformStamped> transforms;
transforms.emplace_back(transform);
SendTransform(transforms);
}
// 批量发布变换
void TransformBroadcaster::SendTransform(
const std::vector<TransformStamped>& transforms) {
auto message = std::make_shared<TransformStampeds>();
*message->mutable_transforms() = {transforms.begin(), transforms.end()};
writer_->Write(message);
}5.3 使用示例
示例1: 在Localization模块中发布车辆位姿
cpp
#include "modules/transform/transform_broadcaster.h"
#include "modules/common/util/message_util.h"
class LocalizationComponent : public cyber::Component<...> {
public:
bool Init() override {
// 创建TransformBroadcaster
tf_broadcaster_ = std::make_unique<TransformBroadcaster>(node_);
return true;
}
void PublishPose(const LocalizationEstimate& localization) {
// 构造world -> localization变换
TransformStamped tf;
// 设置header
apollo::common::util::FillHeader(node_->Name(), tf.mutable_header());
tf.mutable_header()->set_timestamp_sec(localization.measurement_time());
tf.mutable_header()->set_frame_id("world");
// 设置child_frame_id
tf.set_child_frame_id("localization");
// 设置平移(来自定位结果)
tf.mutable_transform()->mutable_translation()->set_x(
localization.pose().position().x());
tf.mutable_transform()->mutable_translation()->set_y(
localization.pose().position().y());
tf.mutable_transform()->mutable_translation()->set_z(
localization.pose().position().z());
// 设置旋转(来自定位结果)
tf.mutable_transform()->mutable_rotation()->set_qx(
localization.pose().orientation().qx());
tf.mutable_transform()->mutable_rotation()->set_qy(
localization.pose().orientation().qy());
tf.mutable_transform()->mutable_rotation()->set_qz(
localization.pose().orientation().qz());
tf.mutable_transform()->mutable_rotation()->set_qw(
localization.pose().orientation().qw());
// 发布变换
tf_broadcaster_->SendTransform(tf);
}
private:
std::unique_ptr<TransformBroadcaster> tf_broadcaster_;
};示例2: 发布动态障碍物坐标系
cpp
// 在Perception模块中,为每个跟踪的障碍物发布坐标系
void PublishObstacleTFs(const std::vector<TrackedObject>& objects) {
std::vector<TransformStamped> transforms;
for (const auto& obj : objects) {
TransformStamped tf;
// Header
tf.mutable_header()->set_timestamp_sec(obj.timestamp());
tf.mutable_header()->set_frame_id("world");
// Child frame: obstacle_<id>
tf.set_child_frame_id("obstacle_" + std::to_string(obj.id()));
// 位置
tf.mutable_transform()->mutable_translation()->CopyFrom(obj.position());
// 姿态
tf.mutable_transform()->mutable_rotation()->CopyFrom(obj.orientation());
transforms.push_back(tf);
}
// 批量发布
if (!transforms.empty()) {
tf_broadcaster_->SendTransform(transforms);
}
}6. 静态坐标变换
6.1 StaticTransformComponent
文件位置: modules/transform/static_transform_component.h
cpp
namespace apollo {
namespace transform {
/**
* @brief 静态坐标变换发布组件
*
* 在系统启动时一次性加载所有传感器外参,
* 并发布到/tf_static channel供其他模块使用。
*/
class StaticTransformComponent final : public apollo::cyber::Component<> {
public:
StaticTransformComponent() = default;
~StaticTransformComponent() = default;
public:
/**
* @brief 组件初始化
* - 加载配置文件
* - 解析YAML外参文件
* - 发布静态变换
*/
bool Init() override;
private:
/**
* @brief 发送所有静态变换
*/
void SendTransforms();
/**
* @brief 发送变换(更新或添加)
* @param msgtf 变换向量
*/
void SendTransform(const std::vector<TransformStamped>& msgtf);
/**
* @brief 从YAML文件解析外参
* @param file_path YAML文件路径
* @param transform 输出变换
* @return 成功返回true
*/
bool ParseFromYaml(const std::string& file_path,
TransformStamped* transform);
private:
apollo::static_transform::Conf conf_; // 配置
std::shared_ptr<cyber::Writer<TransformStampeds>> writer_; // 发布器
TransformStampeds transform_stampeds_; // 缓存的变换
};
CYBER_REGISTER_COMPONENT(StaticTransformComponent)
} // namespace transform
} // namespace apollo6.2 初始化与发布
cpp
// static_transform_component.cc:27-55
bool StaticTransformComponent::Init() {
// 1. 加载配置文件
if (!GetProtoConfig(&conf_)) {
AERROR << "Parse conf file failed, " << ConfigFilePath();
return false;
}
// 2. 创建Writer(QoS: TF_STATIC)
cyber::proto::RoleAttributes attr;
attr.set_channel_name(FLAGS_tf_static_topic); // "/tf_static"
attr.mutable_qos_profile()->CopyFrom(
cyber::transport::QosProfileConf::QOS_PROFILE_TF_STATIC);
writer_ = node_->CreateWriter<TransformStampeds>(attr);
// 3. 发送所有静态变换
SendTransforms();
return true;
}
void StaticTransformComponent::SendTransforms() {
std::vector<TransformStamped> tranform_stamped_vec;
// 遍历配置中的所有外参文件
for (auto& extrinsic_file : conf_.extrinsic_file()) {
if (extrinsic_file.enable()) {
AINFO << "Broadcast static transform, frame id ["
<< extrinsic_file.frame_id() << "], child frame id ["
<< extrinsic_file.child_frame_id() << "]";
// 解析YAML文件
TransformStamped transform;
if (ParseFromYaml(extrinsic_file.file_path(), &transform)) {
tranform_stamped_vec.emplace_back(transform);
}
}
}
// 发布所有变换
SendTransform(tranform_stamped_vec);
}6.3 YAML解析
cpp
// static_transform_component.cc:57-86
bool StaticTransformComponent::ParseFromYaml(
const std::string& file_path,
TransformStamped* transform_stamped) {
// 检查文件是否存在
if (!cyber::common::PathExists(file_path)) {
AERROR << "Extrinsic yaml file does not exist: " << file_path;
return false;
}
// 加载YAML文件
YAML::Node tf = YAML::LoadFile(file_path);
try {
// 解析header
transform_stamped->mutable_header()->set_frame_id(
tf["header"]["frame_id"].as<std::string>());
// 解析child_frame_id
transform_stamped->set_child_frame_id(
tf["child_frame_id"].as<std::string>());
// 解析平移
auto translation =
transform_stamped->mutable_transform()->mutable_translation();
translation->set_x(tf["transform"]["translation"]["x"].as<double>());
translation->set_y(tf["transform"]["translation"]["y"].as<double>());
translation->set_z(tf["transform"]["translation"]["z"].as<double>());
// 解析旋转
auto rotation =
transform_stamped->mutable_transform()->mutable_rotation();
rotation->set_qx(tf["transform"]["rotation"]["x"].as<double>());
rotation->set_qy(tf["transform"]["rotation"]["y"].as<double>());
rotation->set_qz(tf["transform"]["rotation"]["z"].as<double>());
rotation->set_qw(tf["transform"]["rotation"]["w"].as<double>());
} catch (...) {
AERROR << "Extrinsic yaml file parse failed: " << file_path;
return false;
}
return true;
}6.4 YAML外参文件格式
示例: novatel_localization_extrinsics.yaml
yaml
# 父坐标系
header:
seq: 0
stamp:
secs: 1512689414
nsecs: 0
frame_id: localization
# 子坐标系
child_frame_id: novatel
# 变换
transform:
# 平移(米)
translation:
x: 0.0
y: 0.0
z: 0.0
# 旋转(四元数)
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0说明:
translation: novatel相对localization的位置偏移rotation: novatel相对localization的姿态偏移- 单位矩阵旋转(0,0,0,1)表示两个坐标系姿态完全对齐
6.5 配置文件
static_transform_conf.pb.txt
protobuf
# 外参文件1: novatel -> velodyne64
extrinsic_file {
frame_id: "novatel"
child_frame_id: "velodyne64"
file_path: "modules/drivers/lidar/velodyne/params/velodyne64_novatel_extrinsics.yaml"
enable: true
}
# 外参文件2: localization -> novatel
extrinsic_file {
frame_id: "localization"
child_frame_id: "novatel"
file_path: "modules/localization/msf/params/novatel_localization_extrinsics.yaml"
enable: true
}
# 外参文件3: localization -> imu
extrinsic_file {
frame_id: "localization"
child_frame_id: "imu"
file_path: "modules/localization/msf/params/imu_localization_extrinsics.yaml"
enable: true
}
# 外参文件4: velodyne64 -> front_6mm (相机)
extrinsic_file {
frame_id: "velodyne64"
child_frame_id: "front_6mm"
file_path: "modules/perception/data/params/front_6mm_extrinsics.yaml"
enable: true
}
# 外参文件5: velodyne64 -> front_12mm (相机)
extrinsic_file {
frame_id: "velodyne64"
child_frame_id: "front_12mm"
file_path: "modules/perception/data/params/front_12mm_extrinsics.yaml"
enable: true
}
# 外参文件6: velodyne64 -> radar_front (毫米波雷达)
extrinsic_file {
frame_id: "velodyne64"
child_frame_id: "radar_front"
file_path: "modules/perception/data/params/radar_front_extrinsics.yaml"
enable: true
}启用/禁用外参:
protobuf
# 禁用某个传感器的外参
extrinsic_file {
frame_id: "velodyne64"
child_frame_id: "radar_front"
file_path: "..."
enable: false # 设为false禁用
}7. TF2库集成
7.1 TF2库简介
TF2 (Transform Library 2) 是ROS生态中广泛使用的坐标变换库,Apollo继承了其核心功能。
TF2核心特性:
- 树状结构管理坐标系关系
- 时间缓存与插值
- 路径搜索算法
- 线程安全
Apollo对TF2的扩展:
- Protobuf消息格式适配
- CyberRT通信集成
- 单例模式
- 仿真时间支持
7.2 BufferCore核心功能
cpp
namespace tf2 {
/**
* @brief TF2核心类,管理坐标变换树
*/
class BufferCore {
public:
/**
* @brief 构造函数
* @param cache_time 缓存时间(默认10秒)
*/
explicit BufferCore(ros::Duration cache_time = ros::Duration(10.0));
/**
* @brief 添加变换到树中
* @param transform 变换数据
* @param authority 数据来源(用于调试)
* @param is_static 是否为静态变换
* @return 成功返回true
*/
bool setTransform(const geometry_msgs::TransformStamped& transform,
const std::string& authority,
bool is_static = false);
/**
* @brief 查询变换
* @param target_frame 目标坐标系
* @param source_frame 源坐标系
* @param time 查询时间
* @return 变换数据
* @throw tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException
*/
geometry_msgs::TransformStamped lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
const ros::Time& time) const;
/**
* @brief 检查变换是否可用
* @param target_frame 目标坐标系
* @param source_frame 源坐标系
* @param time 查询时间
* @param error_msg 错误信息输出
* @return 可用返回true
*/
bool canTransform(const std::string& target_frame,
const std::string& source_frame,
const ros::Time& time,
std::string* error_msg = nullptr) const;
/**
* @brief 获取所有坐标系名称
* @return 坐标系列表
*/
std::vector<std::string> getAllFrameNames() const;
/**
* @brief 导出TF树为YAML格式(调试用)
* @return YAML字符串
*/
std::string allFramesAsYAML() const;
/**
* @brief 清空所有缓存
*/
void clear();
private:
// 坐标系ID -> TimeCache映射
std::map<CompactFrameID, TimeCache> frames_;
// 坐标系名称 <-> ID映射
std::map<std::string, CompactFrameID> frameIDs_;
std::vector<std::string> frameIDs_reverse_;
// 缓存时间
ros::Duration cache_time_;
// 线程安全锁
mutable boost::mutex frame_mutex_;
};
} // namespace tf27.3 时间缓存(TimeCache)
TF2内部使用TimeCache存储每个坐标系的历史变换:
cpp
namespace tf2 {
/**
* @brief 时间缓存类,存储单个坐标系的变换历史
*/
class TimeCache {
public:
TimeCache(ros::Duration max_storage_time = ros::Duration(10.0));
/**
* @brief 插入新的变换
* @param data 变换数据
* @return 成功返回true
*/
bool insertData(const TransformStorage& data);
/**
* @brief 查询指定时间的变换
* @param time 查询时间
* @param data_out 输出变换
* @return 成功返回true
*/
bool getData(ros::Time time, TransformStorage& data_out);
/**
* @brief 获取最新变换的时间戳
*/
ros::Time getLatestTimestamp();
/**
* @brief 获取最老变换的时间戳
*/
ros::Time getOldestTimestamp();
private:
// 变换历史列表(按时间排序)
std::list<TransformStorage> storage_;
// 最大缓存时间
ros::Duration max_storage_time_;
// 修剪旧数据
void pruneList();
};
/**
* @brief 变换存储结构
*/
struct TransformStorage {
ros::Time stamp; // 时间戳
Quaternion rotation; // 旋转(四元数)
Vector3 translation; // 平移
CompactFrameID frame_id; // 父坐标系ID
CompactFrameID child_frame_id; // 子坐标系ID
};
} // namespace tf2时间缓存示例:
TimeCache for "localization" (parent: "world"):
storage_ = [
{stamp: 1.0s, rotation: q1, translation: t1},
{stamp: 1.01s, rotation: q2, translation: t2},
{stamp: 1.02s, rotation: q3, translation: t3},
...
{stamp: 10.0s, rotation: qN, translation: tN}
]
max_storage_time_ = 10.0s
查询 getData(1.015s):
1. 找到相邻时间戳: 1.01s 和 1.02s
2. 线性插值:
ratio = (1.015 - 1.01) / (1.02 - 1.01) = 0.5
translation = lerp(t2, t3, 0.5)
rotation = slerp(q2, q3, 0.5)
3. 返回插值结果
自动修剪:
当前时间 > 11.0s时, 1.0s的数据被删除7.4 路径搜索算法
TF2使用**广度优先搜索(BFS)**查找坐标系之间的路径:
cpp
// tf2/buffer_core.cpp (伪代码)
bool BufferCore::lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
const ros::Time& time,
geometry_msgs::TransformStamped& result) const {
// 1. 获取坐标系ID
CompactFrameID target_id = lookupFrameNumber(target_frame);
CompactFrameID source_id = lookupFrameNumber(source_frame);
// 2. BFS搜索路径
std::vector<CompactFrameID> path = findPath(source_id, target_id);
if (path.empty()) {
throw ConnectivityException("No path from " + source_frame +
" to " + target_frame);
}
// 3. 沿路径累积变换
TransformStorage accum;
accum.setIdentity();
for (size_t i = 0; i < path.size() - 1; ++i) {
CompactFrameID from_id = path[i];
CompactFrameID to_id = path[i + 1];
// 查询from_id -> to_id的变换
TransformStorage tf;
if (!frames_[to_id].getData(time, tf)) {
throw ExtrapolationException("Transform not available at time");
}
// 累积变换(矩阵乘法)
accum = accum * tf;
}
// 4. 转换为TransformStamped返回
result = transformStorageToMsg(accum, target_frame, source_frame, time);
return true;
}BFS路径搜索示例:
TF树:
world
│
localization
╱ ╲
novatel imu
│
velodyne64
查询: velodyne64 -> imu
BFS搜索:
1. 从velodyne64开始,找到父节点novatel
2. 从novatel找到父节点localization
3. 从localization找到子节点imu
路径: velodyne64 -> novatel -> localization -> imu
变换计算:
T_imu_velo = T_imu_loc * T_loc_nova * T_nova_velo
= (T_loc_imu)^-1 * (T_nova_loc)^-1 * T_nova_velo7.5 插值算法
7.5.1 平移插值(线性插值)
cpp
Vector3 lerp(const Vector3& v1, const Vector3& v2, double ratio) {
return v1 + (v2 - v1) * ratio;
}
示例:
t1 = (1.0, 2.0, 3.0) at time=1.0s
t2 = (2.0, 3.0, 4.0) at time=2.0s
查询 time=1.5s:
ratio = (1.5 - 1.0) / (2.0 - 1.0) = 0.5
result = lerp(t1, t2, 0.5)
= (1.0, 2.0, 3.0) + ((2.0, 3.0, 4.0) - (1.0, 2.0, 3.0)) * 0.5
= (1.5, 2.5, 3.5)7.5.2 旋转插值(球面线性插值SLERP)
cpp
Quaternion slerp(const Quaternion& q1, const Quaternion& q2, double ratio) {
// 1. 计算夹角余弦
double dot = q1.dot(q2);
// 2. 确保走最短路径
if (dot < 0.0) {
q2 = -q2;
dot = -dot;
}
// 3. 线性插值(夹角很小时)
if (dot > 0.9995) {
return normalize(q1 + (q2 - q1) * ratio);
}
// 4. 球面插值
double theta = acos(dot);
double sin_theta = sin(theta);
double ratio1 = sin((1.0 - ratio) * theta) / sin_theta;
double ratio2 = sin(ratio * theta) / sin_theta;
return q1 * ratio1 + q2 * ratio2;
}
示例:
q1 = (0, 0, 0, 1) 表示0度旋转
q2 = (0, 0, 0.707, 0.707) 表示90度绕Z轴旋转
查询 ratio=0.5 (中间姿态):
result = slerp(q1, q2, 0.5)
≈ (0, 0, 0.383, 0.924) 表示45度绕Z轴旋转8. 外参标定
8.1 标定概述
外参标定是确定传感器之间精确空间关系的过程,直接影响多传感器融合精度。
标定精度要求:
| 传感器对 | 平移精度 | 旋转精度 | 备注 |
|---|---|---|---|
| 激光雷达-GNSS | <2cm | <0.5° | 影响定位精度 |
| 激光雷达-相机 | <1cm | <0.2° | 影响感知融合 |
| 激光雷达-雷达 | <2cm | <1° | 影响目标跟踪 |
8.2 标定工具
8.2.1 激光雷达-相机标定
工具: modules/calibration/lidar_camera_calibration
步骤:
bash
# 1. 采集标定数据
cd /apollo
./scripts/calibration_data_collect.sh
# 2. 运行标定算法
./bazel-bin/modules/calibration/lidar_camera_calibration/lidar_camera_calibration \
--bag_file=calibration_data.bag \
--intrinsic_file=camera_intrinsics.yaml \
--output_file=lidar_camera_extrinsics.yaml
# 3. 验证标定结果
./scripts/calibration_visualize.sh \
--extrinsics=lidar_camera_extrinsics.yaml算法原理:
1. 检测标定板角点
- 图像: 使用OpenCV棋盘格检测
- 点云: 平面拟合+边缘检测
2. 建立对应关系
- 3D点(激光雷达) <-> 2D点(图像)
3. 优化外参矩阵
minimize Σ ||project(T * P3D) - P2D||²
其中:
- T: 待求外参矩阵(4x4)
- P3D: 激光雷达中的3D点
- P2D: 图像中的2D点
- project(): 相机投影函数
4. 输出结果
translation: (x, y, z)
rotation: (qx, qy, qz, qw)8.2.2 多激光雷达标定
工具: modules/calibration/multi_lidar_calibration
bash
./bazel-bin/modules/calibration/multi_lidar_calibration/multi_lidar_calibration \
--master_lidar=velodyne64 \
--slave_lidar=velodyne16 \
--bag_file=calibration_data.bag算法: 基于ICP(Iterative Closest Point)点云配准
8.3 标定结果验证
8.3.1 可视化验证
python
# visualize_extrinsics.py
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def visualize_sensor_layout(extrinsics_files):
"""可视化传感器布局"""
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')
# 读取外参
transforms = {}
for file_path in extrinsics_files:
tf = load_extrinsic(file_path)
transforms[tf['child_frame_id']] = tf
# 绘制坐标系
for frame, tf in transforms.items():
pos = tf['translation']
rot = tf['rotation']
# 绘制原点
ax.scatter(pos[0], pos[1], pos[2], s=100, label=frame)
# 绘制坐标轴
axes = rotation_to_axes(rot)
for i, axis in enumerate(axes):
ax.quiver(pos[0], pos[1], pos[2],
axis[0], axis[1], axis[2],
length=0.3, color=['r', 'g', 'b'][i])
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.legend()
plt.title('Sensor Extrinsics Layout')
plt.show()8.3.2 投影精度验证
cpp
// 将激光点投影到图像,检查对齐精度
void VerifyLidarCameraCalibration(
const PointCloud& cloud,
const cv::Mat& image,
const TransformStamped& extrinsic,
const CameraIntrinsic& intrinsic) {
for (const auto& point : cloud.points()) {
// 1. 激光雷达坐标 -> 相机坐标
Eigen::Vector3d p_lidar(point.x(), point.y(), point.z());
Eigen::Vector3d p_camera = TransformPoint(extrinsic, p_lidar);
// 2. 相机坐标 -> 图像坐标
Eigen::Vector2d p_image = Project(p_camera, intrinsic);
// 3. 在图像上绘制投影点
if (IsInImage(p_image, image.size())) {
cv::circle(image, cv::Point(p_image.x(), p_image.y()),
3, cv::Scalar(0, 255, 0), -1);
}
}
cv::imshow("Lidar-Camera Projection", image);
cv::waitKey(0);
}8.4 外参更新流程
bash
# 1. 标定获得新外参
new_extrinsics.yaml
# 2. 备份旧外参
cp modules/perception/data/params/front_6mm_extrinsics.yaml \
modules/perception/data/params/front_6mm_extrinsics.yaml.bak
# 3. 替换外参文件
cp new_extrinsics.yaml \
modules/perception/data/params/front_6mm_extrinsics.yaml
# 4. 重启Transform模块
cyber_launch stop static_transform.launch
cyber_launch start static_transform.launch
# 5. 验证新外参
cyber_monitor -c /tf_static9. 配置与部署
9.1 配置文件
9.1.1 DAG配置
static_transform.dag
protobuf
# modules/transform/dag/static_transform.dag
module_config {
module_library : "modules/transform/libstatic_transform_component.so"
components {
class_name : "StaticTransformComponent"
config {
name : "static_transform"
config_file_path: "modules/transform/conf/static_transform_conf.pb.txt"
}
}
}9.1.2 GFlags配置
cpp
// modules/common/adapters/adapter_gflags.cc
DEFINE_string(tf_topic, "/tf",
"Transform topic");
DEFINE_string(tf_static_topic, "/tf_static",
"Static transform topic");9.2 部署步骤
步骤1: 准备外参文件
bash
# 确保所有外参文件存在
ls modules/localization/msf/params/*extrinsics.yaml
ls modules/perception/data/params/*extrinsics.yaml步骤2: 配置静态变换
bash
# 编辑配置文件
vim modules/transform/conf/static_transform_conf.pb.txt
# 添加/修改外参配置
extrinsic_file {
frame_id: "parent_frame"
child_frame_id: "child_frame"
file_path: "path/to/extrinsics.yaml"
enable: true
}步骤3: 启动Transform模块
bash
# 启动CyberRT
cd /apollo
./scripts/bootstrap.sh
# 启动静态变换组件
cyber_launch start modules/transform/launch/static_transform.launch步骤4: 验证运行
bash
# 检查静态变换是否发布
cyber_monitor
# 查看/tf_static channel,应该看到TransformStampeds消息
# 检查TF树
python3 /apollo/scripts/view_tf_tree.py9.3 性能优化
9.3.1 缓存时间调整
默认缓存10秒的变换历史,可根据需求调整:
cpp
// buffer.cc修改构造函数
Buffer::Buffer()
: BufferCore(ros::Duration(20.0)) { // 增加到20秒
Init();
}9.3.2 QoS配置优化
cpp
// 对于高频变换(>100Hz),调整QoS
apollo::cyber::proto::RoleAttributes attr;
attr.set_channel_name("/tf");
attr.mutable_qos_profile()->set_depth(100); // 增加队列深度9.4 多机部署
在多机分布式部署时,需要配置TF同步:
bash
# 机器A: 发布Transform
cyber_launch start localization.launch
# 机器B: 订阅Transform
# Buffer会自动订阅其他机器发布的/tf和/tf_static10. 调试与故障排查
10.1 常见问题
问题1: Transform lookup失败
错误日志:
[ERROR] Transform from velodyne64 to localization not available
[ERROR] ConnectivityException: No path from velodyne64 to localization可能原因:
- 静态变换未发布
- TF树不连通
- 坐标系名称拼写错误
解决方案:
bash
# 1. 检查静态变换是否发布
cyber_monitor -c /tf_static
# 2. 可视化TF树
python3 /apollo/scripts/view_tf_tree.py
# 3. 检查拼写
# 常见错误: "velodyne_64" vs "velodyne64"
# 4. 重启静态变换组件
cyber_launch restart static_transform.launch问题2: ExtrapolationException (时间戳超出范围)
错误日志:
[ERROR] ExtrapolationException: Transform timestamp 12345.6 is outside buffer range [12335.6, 12345.5]原因: 查询的时间戳超出缓存范围(默认10秒)
解决方案:
cpp
// 方法1: 使用当前时间(不推荐)
auto tf = buffer->lookupTransform(
"localization", "velodyne64",
cyber::Time(0) // 0表示最新时间
);
// 方法2: 增加缓存时间
// 修改buffer.cc构造函数
Buffer::Buffer() : BufferCore(ros::Duration(20.0)) { ... }
// 方法3: 检查数据时间戳是否正确
AINFO << "Pointcloud timestamp: " << cloud->header().timestamp_sec();
AINFO << "Current time: " << cyber::Time::Now().ToSecond();问题3: 外参文件解析失败
错误日志:
[ERROR] Extrinsic yaml file parse failed: modules/perception/data/params/front_6mm_extrinsics.yaml解决方案:
bash
# 检查YAML语法
python3 -c "import yaml; yaml.safe_load(open('front_6mm_extrinsics.yaml'))"
# 检查必需字段
# 必须包含: header.frame_id, child_frame_id, transform.translation, transform.rotation
# 检查文件权限
ls -l modules/perception/data/params/front_6mm_extrinsics.yaml问题4: 坐标系反向问题
现象: 查询A->B成功,但查询B->A失败
原因: TF树只存储单向关系,但Buffer支持双向查询
调试:
cpp
// 检查坐标系关系
std::string error;
if (!buffer->canTransform("B", "A", time, 0.01f, &error)) {
AERROR << "Transform B->A not available: " << error;
// 尝试反向查询
auto tf_A2B = buffer->lookupTransform("A", "B", time);
// 手动求逆
auto tf_B2A = InverseTransform(tf_A2B);
}10.2 调试工具
10.2.1 cyber_monitor
bash
# 实时监控Transform消息
cyber_monitor
# 查看/tf channel (动态变换)
# 查看/tf_static channel (静态变换)
# 检查消息频率和内容10.2.2 view_tf_tree.py
python
#!/usr/bin/env python3
"""可视化TF树结构"""
from cyber.python.cyber_py3 import cyber
from modules.transform import Buffer
import networkx as nx
import matplotlib.pyplot as plt
def visualize_tf_tree():
cyber.init()
buffer = Buffer.Instance()
# 等待Buffer初始化
time.sleep(2.0)
# 获取所有坐标系
frames = buffer.getAllFrameNames()
# 构建图
G = nx.DiGraph()
for frame in frames:
G.add_node(frame)
# 获取父坐标系
parent = buffer.getParent(frame, cyber.Time::Now())
if parent:
G.add_edge(parent, frame)
# 绘制
pos = nx.spring_layout(G)
nx.draw(G, pos, with_labels=True, node_size=2000,
node_color='lightblue', arrows=True)
plt.title('TF Tree Structure')
plt.show()
cyber.shutdown()
if __name__ == '__main__':
visualize_tf_tree()10.2.3 tf_echo工具
bash
# 实时打印两个坐标系之间的变换
cyber_launch start modules/tools/tf_echo/tf_echo.launch \
--source=velodyne64 \
--target=localization10.3 性能分析
10.3.1 查询耗时统计
cpp
#include "cyber/common/time.h"
auto start = cyber::Time::Now();
auto tf = buffer->lookupTransform("localization", "velodyne64", time);
auto end = cyber::Time::Now();
double duration_ms = (end - start).ToSecond() * 1000.0;
AINFO << "Transform lookup took " << duration_ms << " ms";典型耗时:
- 简单查询(直接父子关系): <0.1ms
- 多跳查询(3-4层): 0.1-0.5ms
- 带时间插值: 0.2-1.0ms
10.3.2 内存使用分析
bash
# 使用pmap查看内存
ps aux | grep static_transform_component
pmap -x <PID> | grep -E "total|heap"典型内存占用:
- Buffer (10秒缓存, 10个坐标系, 100Hz): ~50MB
- Static变换: <1MB
10.4 故障排查清单
□ 检查Transform模块是否运行
cyber_monitor (查看static_transform节点)
□ 检查静态变换是否发布
cyber_monitor -c /tf_static
□ 检查动态变换是否发布
cyber_monitor -c /tf
□ 验证坐标系名称拼写
使用view_tf_tree.py查看所有坐标系
□ 检查外参文件是否存在
ls modules/*/params/*extrinsics.yaml
□ 验证外参文件格式
python3 -c "import yaml; yaml.safe_load(open('...'))"
□ 检查时间戳是否合理
确保查询时间在缓存范围内(最近10秒)
□ 验证TF树连通性
确保从source到target有路径
□ 检查QoS配置
静态变换是否使用TRANSIENT_LOCAL
□ 查看错误日志
cat /apollo/data/log/static_transform_component.ERROR参考资料与引用
官方资源
-
Apollo 官方 GitHub 仓库
https://github.com/ApolloAuto/apollo
- Apollo 开源项目主仓库,包含完整源代码
-
Apollo 官方文档
- 官方技术文档和开发指南
-
Apollo 开发者社区
https://apollo.baidu.com/community
- 官方开发者论坛和技术交流平台
技术规范与标准
-
ISO 26262 - 道路车辆功能安全标准
-
ISO 21448 (SOTIF) - 预期功能安全标准
学术论文与技术资源
-
CenterPoint: Center-based 3D Object Detection and Tracking
Yin, T., Zhou, X., & Krähenbühl, P. (2021)
-
BEVFormer: Learning Bird's-Eye-View Representation from Multi-Camera Images
Li, Z., et al. (2022)
-
OpenDRIVE 地图标准
开源工具与库
-
Bazel 构建系统
-
Fast-DDS (eProsima)
https://www.eprosima.com/index.php/products-all/eprosima-fast-dds
-
PROJ 坐标转换库
-
TensorRT 开发指南
-
PCL 点云库文档
-
IPOPT 优化求解器
说明
本文档内容整理自上述官方资料、开源代码以及相关技术文档。所有代码示例和技术细节均基于 Apollo 9.0/10.0 版本。如需获取最新信息,请访问 Apollo 官方网站和 GitHub 仓库。
版权说明
Apollo® 是百度公司的注册商标。本文档为基于开源项目的非官方技术研究文档,仅供学习参考使用。