使用 Apollo TransformWrapper 生成相机到各坐标系的变换矩阵

- 一、背景
- 二、原理
- - 1、什么是变换矩阵？
  - 2、为什么需要变换矩阵？
  - 3、Apollo 中的坐标系
  - 4、Apollo TransformWrapper
- 三、操作步骤
- - 1. 设置车辆参数
  - 2. 启动静态变换发布
  - 3. 查看变换信息
  - 4. 播放记录数据生成动态变换
  - 5. 打印TF树
  - 6. 生成相机到各坐标系的变换矩阵
  - - 6.1 编写代码
    - 6.2 编译代码
    - 6.3 运行程序
- 四、小结
- - 1、关键点：

一、背景

在自动驾驶系统中，车辆配备了多种传感器，如相机、激光雷达（LiDAR）、毫米波雷达和定位设备（如 Novatel）。这些传感器各自采集的数据需要统一到同一个坐标系下，才能进行融合处理和后续的感知、定位与决策。例如，相机捕获的图像需要与激光雷达的点云数据进行对齐，从而更准确地识别障碍物或理解环境。

由于每个传感器安装在车辆的不同位置，它们都有自己的局部坐标系。为了将数据统一，我们需要知道这些坐标系之间的变换关系，即变换矩阵。变换矩阵描述了如何将一个坐标系中的点转换到另一个坐标系中，包括旋转和平移。

Apollo 平台提供了 TransformWrapper 工具来方便地获取和管理这些变换关系。本文将介绍如何使用该工具生成相机到其他坐标系（如世界坐标系、车辆坐标系、激光雷达坐标系）的变换矩阵。

二、原理

1、什么是变换矩阵？

变换矩阵（Transformation Matrix）是一个4x4的矩阵，用于描述三维空间中的旋转和平移变换。它可以将一个点或向量从一个坐标系转换到另一个坐标系。矩阵的上左3x3部分表示旋转，右上3x1部分表示平移，最后一行通常是[0, 0, 0, 1]。

2、为什么需要变换矩阵？

在自动驾驶中，不同传感器产生的数据需要在一个统一的坐标系下进行处理。例如：

相机：提供图像数据，但在图像中我们只知道像素坐标，需要知道物体在三维世界中的位置。
激光雷达：提供三维点云，但需要与图像数据融合，以更好地识别物体。
定位设备（如 Novatel）：提供车辆在全球坐标系（如世界坐标系）中的位置和姿态。

通过变换矩阵，我们可以将相机数据转换到世界坐标系，或者将激光雷达数据转换到相机坐标系，从而实现多传感器数据的融合和统一处理。

3、Apollo 中的坐标系

在 Apollo 系统中，常见的坐标系包括：

世界坐标系（world）：全局坐标系，通常基于地图或GPS。
车辆坐标系（novatel）：以车辆定位设备（如Novatel）为原点的坐标系。
激光雷达坐标系（velodyne64）：以激光雷达传感器为原点的坐标系。
相机坐标系（如 front_6mm）：以相机为原点的坐标系。

4、Apollo TransformWrapper

TransformWrapper 是 Apollo 平台提供的一个工具类，用于方便地获取不同坐标系之间的变换关系。它内部使用 TF（Transform Library）来管理和查询坐标系之间的变换。

三、操作步骤

1. 设置车辆参数

首先，我们需要将车辆的传感器参数文件复制到 Apollo 系统的指定目录。这些参数文件描述了传感器的安装位置和外参（即相对于车辆坐标系的变换）。

# 清除现有的参数目录
rm -rf /apollo/modules/perception/data/params/# 复制相机参数
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/camera_params/* /apollo/modules/perception/data/params/# 复制激光雷达参数
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/lidar_params/* /apollo/modules/perception/data/params/# 复制其他参数文件
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/*.txt /apollo/modules/perception/data/params/# 复制静态变换配置
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/transform_conf/static_transform_conf.pb.txt  \/apollo/modules/transform/conf/static_transform_conf.pb.txt# 复制激光雷达外参
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/lidar_params/velodyne64_novatel_extrinsics.yaml \/apollo/modules/drivers/lidar/velodyne/params/velodyne64_novatel_extrinsics.yaml# 复制定位设备外参
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/novatel_localization_extrinsics.yaml \/apollo/modules/localization/msf/params/novatel_localization_extrinsics.yaml# 复制雷达外参
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/radar_params/radar_front_extrinsics.yaml \/apollo/modules/perception/data/params/# 复制传感器元数据
rm -f /apollo/modules/perception/data/conf/sensor_meta.pb.txt	
cp -vf /opt/apollo/neo/data/calibration_data/mkz_121/sensor_meta.pb.txt \/apollo/modules/perception/data/conf/sensor_meta.pb.txt

2. 启动静态变换发布

静态变换是指传感器坐标系之间固定不变的变换关系（如安装位置决定的变换）。启动 static_transform 模块可以发布这些静态变换到 /tf_static topic。

# 终止所有现有进程
bash kill_all.sh# 设置日志级别
export GLOG_minloglevel=1
export GLOG_v=1
export GLOG_alsologtostderr=1# 启动静态变换发布模块
mainboard -d /apollo/modules/transform/dag/static_transform.dag

3. 查看变换信息

使用 cyber_monitor 工具可以查看当前发布的变换信息，例如 /tf_static topic 中的静态变换。

cyber_monitor

在输出中，可以看到类似以下的内容：

ChannelName: /tf_static
MessageType: apollo.transform.TransformStampeds
FrameRatio: 0.00
transforms: [0]header:frame_id: novatelchild_frame_id: velodyne64transform:translation:x: 0.000000000y: 0.414000000z: 0.897000000rotation:qx: 0.000000000qy: 0.000000000qz: 0.707100000qw: 0.707100000ChannelName: /tf
MessageType: apollo.transform.TransformStampeds
FrameRatio: 193.20
transforms: [0]header:timestamp_sec: 1513807877.710000038sequence_num: 0frame_id: worldchild_frame_id: localizationtransform:translation:x: 587121.863165810y: 4141199.892281929z: -31.851399446rotation:qx: -0.019235064qy: 0.041703123qz: -0.787122706qw: 0.615084310

这表示从 novatel 坐标系到 velodyne64 坐标系的变换：平移 (0, 0.414, 0.897) 和旋转（四元数形式）。

4. 播放记录数据生成动态变换

动态变换（如车辆在世界坐标系中的位姿）通常来自定位设备或记录数据。我们可以播放记录文件来生成 /tf topic。

cyber_recorder play -f sensor_rgb.record -l -c /tf

5. 打印TF树

为了直观地查看所有坐标系之间的变换关系，我们使用以下 Python 脚本打印TF树。

cat > dump_tf_tree.py << 'EOF'
import sys
sys.path.append("/opt/apollo//neo/python/cyber/python")
sys.path.append("/opt/apollo/neo/python")
from cyber_py3 import cyber
from modules.common_msgs.transform_msgs import transform_pb2
import timetf_static_received = False
tf_received = False
links = set() 
def tf_static_callback(msg):global tf_static_received, linkstf_static_received = True    for transform in msg.transforms:link = f"{transform.child_frame_id}-->{transform.header.frame_id}"links.add(link)def tf_callback(msg):global tf_received, linkstf_received = True    for transform in msg.transforms:link = f"{transform.child_frame_id}-->{transform.header.frame_id}"links.add(link)def print_mermaid_graph():print("graph TD")for link in links:print(f"    {link}")
def main():cyber.init()node = cyber.Node("test")    node.create_reader("/tf_static",transform_pb2.TransformStampeds,tf_static_callback)node.create_reader("/tf",transform_pb2.TransformStampeds,tf_callback)        while cyber.ok():if tf_static_received and tf_received:breaktime.sleep(1)if links:print_mermaid_graph()    cyber.shutdown()if __name__ == "__main__":main()
EOF
python3 dump_tf_tree.py

输出

该图表示坐标系之间的变换关系，例如 front_6mm（相机）到 velodyne64（激光雷达）的变换。

6. 生成相机到各坐标系的变换矩阵

我们使用 C++ 程序调用 TransformWrapper 来获取相机到其他坐标系的变换矩阵。

6.1 编写代码

cat > apollo_tf_trans.cpp << 'EOF'
#include <iostream>
#include <memory>
#include <fstream>
#include <Eigen/Dense>
#include "cyber/cyber.h"
#include "modules/perception/common/onboard/transform_wrapper/transform_wrapper.h"// 函数用于获取变换并输出信息
bool getTransform(apollo::perception::onboard::TransformWrapper& trans_wrapper,double timestamp,Eigen::Affine3d& transform,const std::string& target_frame = "",const std::string& source_frame = "") {while (true) {bool success;if (target_frame.empty() || source_frame.empty()) {success = trans_wrapper.GetSensor2worldTrans(timestamp, &transform);} else {success = trans_wrapper.GetTrans(timestamp, &transform, target_frame, source_frame);}if (success) {return true;}AERROR << "Failed to get transform, timestamp: " << timestamp;std::this_thread::sleep_for(std::chrono::milliseconds(10));}
}// 函数用于打印和保存变换信息
void printAndSaveTransform(std::ofstream& out_file,const Eigen::Affine3d& transform,const std::string& description) {auto matrix = transform.matrix();Eigen::Matrix3d rotation = matrix.block<3, 3>(0, 0);Eigen::Vector3d translation = matrix.block<3, 1>(0, 3);Eigen::Quaterniond quat(rotation);std::cout << "-------------------" << description << "-----------------------" << std::endl;std::cout << matrix << std::endl;std::cout << "\n旋转矩阵：" << std::endl;std::cout << rotation << std::endl;std::cout << "\n平移向量：" << std::endl;std::cout << translation.transpose() << std::endl;std::cout << "\n四元数 (qw, qx, qy, qz): ("<< quat.w() << ", " << quat.x() << ", "<< quat.y() << ", " << quat.z() << ")" << std::endl;// 保存到文件out_file << "-------------------" << description << "-----------------------" << std::endl;out_file << "平移向量:" << translation.transpose() << std::endl;out_file << "四元数 (qw, qx, qy, qz):"<< quat.w() << " " << quat.x() << " "<< quat.y() << " " << quat.z() << std::endl << std::endl;
}int run(std::string camera_name)
{std::ofstream out_file(camera_name + "_transform.txt");if (!out_file.is_open()) {AERROR << "无法打开文件用于保存变换信息: " << camera_name + "_transform.txt";return -1;}std::shared_ptr<apollo::perception::onboard::TransformWrapper> trans_wrapper;trans_wrapper.reset(new apollo::perception::onboard::TransformWrapper());trans_wrapper->Init(camera_name);double timestamp = 0;auto tf_buffer = apollo::transform::Buffer::Instance();std::string err_string;if (!tf_buffer->canTransform("novatel", camera_name,apollo::cyber::Time(timestamp), 10.00f, &err_string)) {AERROR << "Transform not available: " << err_string;}// 获取camera到world的变换Eigen::Affine3d camera2world;if (getTransform(*trans_wrapper, timestamp, camera2world)) {printAndSaveTransform(out_file, camera2world, "camera2world");}// 获取camera到novatel的变换Eigen::Affine3d camera2novatel;if (getTransform(*trans_wrapper, timestamp, camera2novatel, "novatel", camera_name)) {printAndSaveTransform(out_file, camera2novatel, "camera2novatel");}// 获取camera到lidar的变换Eigen::Affine3d camera2lidar;if (getTransform(*trans_wrapper, timestamp, camera2lidar, "velodyne64", camera_name)) {printAndSaveTransform(out_file, camera2lidar, "camera2lidar");}out_file.close();std::cout << "变换信息已保存到: " << camera_name + "_transform.txt" << std::endl;std::cout << "-----------------END----------------------" << std::endl;    return 0;
}int main() {std::cout << "=== 使用TransformWrapper生成CAM_FRONT到Global变换矩阵 ===" << std::endl;apollo::cyber::Init("test");    std::string camera_name = "front_6mm";run(camera_name);return 0;
}
EOF

6.2 编译代码

export BAZEL_ID=679551712d2357b63e6e0ce858ebf90e
export BAZEL_OUT_DIR=/apollo_workspace/.cache/bazel/$BAZEL_ID/execroot/apollo-park-generic/bazel-out/aarch64-opt/bin
g++ -std=c++14 -o apollo_tf_trans apollo_tf_trans.cpp -I /apollo_workspace/.cache/bazel/$BAZEL_ID/external/eigen \-I /apollo_workspace \-I /apollo_workspace/.cache/bazel/$BAZEL_ID/external/com_google_protobuf/src \-I $BAZEL_OUT_DIR/external/fastrtps/_virtual_includes/fastrtps \-I $BAZEL_OUT_DIR \-I $BAZEL_OUT_DIR/external/tf2/_virtual_includes/tf2 \/opt/apollo/neo/packages/3rd-protobuf/latest/lib/libprotobuf.so -lpthread \/opt/apollo/neo/lib/modules/common_msgs/sensor_msgs/lib_sensor_image_proto_mcs_bin.so \/opt/apollo/neo/lib/cyber/transport/libcyber_transport.so \/opt/apollo/neo/lib/cyber/service_discovery/libcyber_service_discovery.so \/opt/apollo/neo/lib/cyber/service_discovery/libcyber_service_discovery_role.so \/opt/apollo/neo/lib/cyber/class_loader/shared_library/libshared_library.so \/opt/apollo/neo/lib/cyber/class_loader/utility/libclass_loader_utility.so \/opt/apollo/neo/lib/cyber/class_loader/libcyber_class_loader.so \/opt/apollo/neo/lib/cyber/message/libcyber_message.so \/opt/apollo/neo/lib/cyber/plugin_manager/libcyber_plugin_manager.so \/opt/apollo/neo/lib/cyber/profiler/libcyber_profiler.so \/opt/apollo/neo/lib/cyber/common/libcyber_common.so \/opt/apollo/neo/lib/cyber/data/libcyber_data.so \/opt/apollo/neo/lib/cyber/logger/libcyber_logger.so \/opt/apollo/neo/lib/cyber/service/libcyber_service.so \/opt/apollo/neo/lib/cyber/libcyber.so \/opt/apollo/neo/lib/cyber/timer/libcyber_timer.so \/opt/apollo/neo/lib/cyber/blocker/libcyber_blocker.so \/opt/apollo/neo/lib/cyber/component/libcyber_component.so \/opt/apollo/neo/lib/cyber/tools/cyber_recorder/librecorder.so \/opt/apollo/neo/lib/cyber/base/libcyber_base.so \/opt/apollo/neo/lib/cyber/sysmo/libcyber_sysmo.so \/opt/apollo/neo/lib/cyber/croutine/libcyber_croutine.so \/opt/apollo/neo/lib/cyber/libcyber_binary.so \/opt/apollo/neo/lib/cyber/io/libcyber_io.so \/opt/apollo/neo/lib/cyber/event/libcyber_event.so \/opt/apollo/neo/lib/cyber/statistics/libapollo_statistics.so \/opt/apollo/neo/lib/cyber/scheduler/libcyber_scheduler.so \/opt/apollo/neo/lib/cyber/record/libcyber_record.so \/opt/apollo/neo/lib/cyber/libcyber_state.so \/opt/apollo/neo/lib/cyber/context/libcyber_context.so \/opt/apollo/neo/lib/cyber/node/libcyber_node.so \/opt/apollo/neo/lib/cyber/task/libcyber_task.so \/opt/apollo/neo/lib/cyber/parameter/libcyber_parameter.so \/opt/apollo/neo/lib/cyber/time/libcyber_time.so \/opt/apollo/neo/lib/cyber/transport/libcyber_transport.so \/opt/apollo/neo/lib/cyber/proto/lib_qos_profile_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_topology_change_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_component_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_unit_test_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_record_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_parameter_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_cyber_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_role_attributes_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_transport_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_scheduler_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_run_mode_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_classic_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_dag_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_choreography_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_simple_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_perf_conf_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_clock_proto_cp_bin.so \/opt/apollo/neo/lib/cyber/proto/lib_proto_desc_proto_cp_bin.so \/usr/local/lib/libbvar.so \$BAZEL_OUT_DIR/modules/transform/libapollo_transform.so \$BAZEL_OUT_DIR/modules/perception/common/onboard/libapollo_perception_common_onboard.so \/opt/apollo/neo/packages/3rd-glog/latest/lib/libglog.so \/opt/apollo/neo/packages/3rd-gflags/latest/lib/libgflags.so -lz

6.3 运行程序

export GLOG_minloglevel=7
export GLOG_v=1
export GLOG_alsologtostderr=1
./apollo_tf_trans
cat front_6mm_transform.txt

输出

-------------------camera2world-----------------------
平移向量:     586987 4.14134e+06    -30.6403
四元数 (qw, qx, qy, qz):-0.0799855 0.096477 0.732693 -0.668908-------------------camera2novatel-----------------------
平移向量:0.0999981   1.08399   0.37701
四元数 (qw, qx, qy, qz):0.707114 -0.707086 0 0-------------------camera2lidar-----------------------
平移向量: 0.67  -0.1 -0.52
四元数 (qw, qx, qy, qz):-0.5 0.5 -0.5 0.5