Apollo5.5源码学习笔记（五）transform模块Apollo中的坐标系统详解
本系列博客旨在记录⾃⼰在学习百度⽆⼈驾驶开源框架Apollo的⼼得和体会，欢迎⼤家阅读和点赞，并提出宝贵意见，⼤家相互学习，如需转载，请注明出处，谢谢！
⽂章⽬录
福字怎么剪
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、
2、
3、传感器外参标定关系
了解了Apollo⾃动驾驶车辆上整体的传感器布局后，接下来我们就来看下这些传感器所定义的坐标系以及他们的之间的转换关系，其实也就是标定结果中的frame_id和child_frame_id关系。
对于标定结果的描述，常⽤刚体变换中的旋转变换和平移变换来表⽰，其中涉及到child_frame_id表⽰该传感器数据所在的坐标
系，frame_id则表⽰该标定结果描述了从child_frame_id到frame_id的变换关系。
下⾯的坐标系变换关系图中箭头起点表⽰child_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_⽂件。
为了⽅便分析，下⾯贴出static_transform_⽂件的内容：
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
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到world坐标系的变换关系。
发布函数LocalizationMsgPublisher::PublishPoBroadcastTF定义：
void LocalizationMsgPublisher::PublishPoBroadcastTF(
const LocalizationEstimate& localization){
// broadcast tf message
apollo::transform::TransformStamped tf2_msg;
auto mutable_head = tf2_msg.mutable_header();
mutable_head->t_timestamp_asurement_time());
mutable_head->t_frame_id(broadcast_tf_frame_id_);
tf2_msg.t_child_frame_id(broadcast_tf_child_frame_id_);
auto mutable_translation = tf2_msg.mutable_transform()->mutable_translation();
mutable_translation->t_x(localization.po().position().x());
mutable_translation->t_y(localization.po().position().y());
mutable_translation->t_z(localization.po().position().z());
auto mutable_rotation = tf2_msg.mutable_transform()->mutable_rotation();
mutable_rotation->t_qx(localization.po().orientation().qx());
mutable_rotation->t_qy(localization.po().orientation().qy());
吴根友mutable_rotation->t_qz(localization.po().orientation().qz());
mutable_rotation->t_qw(localization.po().orientation().qw());
tf2_broadcaster_.SendTransform(tf2_msg);
}
其中broadcast_tf_frame_id_和broadcast_tf_child_frame_id_定义在modules/localization/common/⽂件中：
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 po 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;
}
