Apollo 5.5 源码学习笔记（五） | transform模块

本系列博客旨在记录自己在学习百度无人驾驶开源框架Apollo的心得和体会，欢迎大家阅读和点赞，并提出宝贵意见，大家相互学习，如需转载，请注明出处，谢谢！

文章目录

1、前言
2、车辆传感器布局
3、传感器外参标定关系
4、具体实现细节
- 4.1 外参标定结果的管理与维护
- 4.2 本车全局位姿的发布
- 4.3 坐标变换树的应用

1、前言

对于一个自动驾驶系统来说，一般都会有激光雷达、毫米波雷达、超声波雷达、相机、IMU/GPS等各种传感器，而每个传感器都有自己的坐标系定义，为了描述自动驾驶车辆在地图中的位置，一般还涉及到全局定位，这就还会包含全局坐标系，也称为世界坐标系。这些错综复杂的坐标系定义，不仅涉及到了不同传感器之间的内外参标定关系，也包含了许多与坐标系转换之间的操作，需要我们重点掌握和理解。

由于来自不同传感器的数据都是带有坐标系，即数据的表示其实都是某个特定的坐标系的，不同传感器之间的外参标定等内容本博客暂时不说，本篇博客的主要目的是带大家整体理解和掌握Apollo中涉及到的各种坐标系定义和使用注意事项，这能帮我们在阅读Apollo源码特别是感知部分代码时快速理解各种坐标变换的含义和目的。

注：本篇博客主要是对Apollo 5.5的感知模块相关坐标系进行分析，Apollo 5.0应该差别不大。

2、车辆传感器布局

首先我们整体来看一下Apollo官方提供的传感器布置图：

从上面这张图中我们可以看出Apollo的Lincoln MKZ自动驾驶车辆上在车上不同位置布置有数量不同、种类繁多的传感器。

光从上面这张俯视图中我们可能没法清楚看出不同传感器的安装位置及坐标系定义，我们可以看下面这张图：

注意区分其中相机坐标系和激光雷达坐标系的方向定义，其中激光雷达坐标系X轴向前，Y轴向左，Z轴向上，而相机坐标系Z轴向前，X轴向右，Y轴向下。另外在车的中间位置有一个紫色的传感器，上面写着Novatel，其实是诺瓦泰的GPS接收机，这个传感器的坐标系非常关键，因为Apollo中大部分传感器都是标定在该传感器坐标系下，Novatel坐标系定义为X轴向右、Y轴向前、Z轴向上。
【参考】：
1、Apollo Architecture
2、

3、传感器外参标定关系

了解了Apollo自动驾驶车辆上整体的传感器布局后，接下来我们就来看下这些传感器所定义的坐标系以及他们的之间的转换关系，其实也就是标定结果中的frame_id和child_frame_id关系。

对于标定结果的描述，常用刚体变换中的旋转变换和平移变换来表示，其中涉及到child_frame_id表示该传感器数据所在的坐标系，frame_id则表示该标定结果描述了从child_frame_id到frame_id的变换关系。

下面的坐标系变换关系图中箭头起点表示child_frame_id、箭头所指的为frame_id。

从图中可以看出，Apollo将激光雷达、相机都标定到了novatel坐标系下，将毫米波雷达坐标系标定到了velodyne128坐标系下，将novatel坐标系标定到了localization坐标系下，最终通过localization模块发布了从localization到world坐标系的变换，即发布了车辆的全局位姿，这样这些坐标系变换关系就构成了一个变换树（tf_tree），我们可以从这颗变换树中查询任意两个坐标系之间的变换关系，从而完成坐标转换工作。

4、具体实现细节

理解了传感器的布局以及Apollo中对这些传感器所完成的标定任务之后，下一个关键的问题就是Apollo中是如何管理和维护这些传感器外参的标定关系以及本车的全局位姿信息呢？又是如何在程序中实现方便查询这些变换关系，完成我们想要的坐标变换呢？

4.1 外参标定结果的管理与维护

我们知道对于不同传感器之间的外参变换关系来说，由于它们都是固连在自动驾驶车辆上而且在车辆运动过程中它们的相对位置并没有发生变化，所以它们的变换关系是固定不变的。对于拥有这种特征的变换关系，在ROS中我们可以发布tf_static话题，在Apollo中也借用了ROS的这种机制。

Apollo中使用transform模块完成对传感器外参标定结果的发布工作。Component组件类为StaticTransformComponent，配置参数为modules/transform/conf/static_transform_conf.pb.txt文件。

为了方便分析，下面贴出static_transform_conf.pb.txt文件的内容：

extrinsic_file {frame_id: "novatel"child_frame_id: "velodyne128"file_path: "/apollo/modules/drivers/velodyne/params/velodyne128_novatel_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "localization"child_frame_id: "novatel"file_path: "/apollo/modules/localization/msf/params/novatel_localization_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "velodyne128"child_frame_id: "front_6mm"file_path: "/apollo/modules/perception/data/params/front_6mm_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "velodyne128"child_frame_id: "front_12mm"file_path: "/apollo/modules/perception/data/params/front_12mm_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "novatel"child_frame_id: "lidar16_front_up"file_path: "/apollo/modules/drivers/velodyne/params/lidar16_front_up_novatel_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "novatel"child_frame_id: "lidar16_front"file_path: "/apollo/modules/drivers/velodyne/params/lidar16_front_novatel_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "novatel"child_frame_id: "lidar16_rear_left"file_path: "/apollo/modules/drivers/velodyne/params/lidar16_rear_left_novatel_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "novatel"child_frame_id: "lidar16_rear_right"file_path: "/apollo/modules/drivers/velodyne/params/lidar16_rear_right_novatel_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "velodyne128"child_frame_id: "radar_front"file_path: "/apollo/modules/perception/data/params/radar_front_extrinsics.yaml"enable: true
}extrinsic_file {frame_id: "velodyne128"child_frame_id: "radar_rear"file_path: "/apollo/modules/perception/data/params/radar_rear_extrinsics.yaml"enable: true
}

从中可以看出该配置文件定义了StaticTransformComponent类需要发布的所有传感器外参标定关系，并指明了这些具体标定参数yaml文件所在路径。

在StaticTransformComponent类中依次对这些yaml文件进行解析，并发布所有这些静态变换，从而建立起了一棵变换树（tf_tree）。

4.2 本车全局位姿的发布

除了transform模块中的StaticTransformComponent类发布所有传感器的外参变换关系外，还有一个很重要的变换关系需要发布，那就是本车的全局位姿变换关系，这也是定位模块localization主要的任务。

Apollo中通过localization模块中的LocalizationMsgPublisher类（modules/localization/msf/msf_localization_component.cc）完成本车全局位姿的发布工作，其实发布的是localization到world坐标系的变换关系。

发布函数LocalizationMsgPublisher::PublishPoseBroadcastTF定义：

void LocalizationMsgPublisher::PublishPoseBroadcastTF(const LocalizationEstimate& localization) {// broadcast tf messageapollo::transform::TransformStamped tf2_msg;auto mutable_head = tf2_msg.mutable_header();mutable_head->set_timestamp_sec(localization.measurement_time());mutable_head->set_frame_id(broadcast_tf_frame_id_);tf2_msg.set_child_frame_id(broadcast_tf_child_frame_id_);auto mutable_translation = tf2_msg.mutable_transform()->mutable_translation();mutable_translation->set_x(localization.pose().position().x());mutable_translation->set_y(localization.pose().position().y());mutable_translation->set_z(localization.pose().position().z());auto mutable_rotation = tf2_msg.mutable_transform()->mutable_rotation();mutable_rotation->set_qx(localization.pose().orientation().qx());mutable_rotation->set_qy(localization.pose().orientation().qy());mutable_rotation->set_qz(localization.pose().orientation().qz());mutable_rotation->set_qw(localization.pose().orientation().qw());tf2_broadcaster_.SendTransform(tf2_msg);
}

其中broadcast_tf_frame_id_和broadcast_tf_child_frame_id_定义在modules/localization/common/localization_gflags.cc文件中：

DEFINE_string(broadcast_tf_frame_id, "world", "world frame id in tf");
DEFINE_string(broadcast_tf_child_frame_id, "localization","localization frame id in tf");

至此，这棵完整的坐标系变换树就建立起来了。接下来我们以感知模块为例来看下如何利用这棵建立起来的坐标变换树来完成我们需要的坐标变换。

4.3 坐标变换树的应用

建立好了坐标变换树之后，接下来的问题就是如何利用这棵变换树来完成坐标变换工作。

此处以感知模块为例，在感知模块中定义了TransformWrapper类（modules/perception/onboard/transform_wrapper/transform_wrapper.h），用来查询不同坐标系之间的变换矩阵，从而完成坐标变换。

在TransformWrapper类中，主要是通过成员变量Buffer* tf2_buffer_ = Buffer::Instance();在TransformWrapper::QueryTrans函数中完成坐标的查询工作，而Buffer类定义在transform模块下的modules/transform/buffer.h文件中，该类继承自tf2::BufferCore，到这就可以大概清楚了，Apollo其实是借鉴了ROS中tf2的部分实现，来完成对坐标变换关系的缓存和查询工作的。

下面以radar感知来看下TransformWrapper类的具体应用：
RadarDetectionComponent类（modules/perception/onboard/component/radar_detection_component.h）中定义了如下成员变量：

TransformWrapper radar2world_trans_;
TransformWrapper radar2novatel_trans_;

分别用来查询radar到world坐标系之间的变换，以及radar到novatel坐标系之间的变换；

在RadarDetectionComponent::Init函数中初始化该类：

radar2world_trans_.Init(tf_child_frame_id_);
radar2novatel_trans_.Init(tf_child_frame_id_);

其中tf_child_frame_id_为radar_front或radar_rear，这就与上述的Apollo建立的外参标定关系图对应上了，最后我们在RadarDetectionComponent::InternalProc函数中调用TransformWrapper类的相关成员函数根据当前消息的时间戳进行坐标变换矩阵的获取。

Eigen::Affine3d radar_trans;
if (!radar2world_trans_.GetSensor2worldTrans(timestamp, &radar_trans)) {out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;AERROR << "Failed to get pose at time: " << timestamp;return true;
}
Eigen::Affine3d radar2novatel_trans;
if (!radar2novatel_trans_.GetTrans(timestamp, &radar2novatel_trans, "novatel",tf_child_frame_id_)) {out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;AERROR << "Failed to get radar2novatel trans at time: " << timestamp;return true;
}