使用 Apollo TransformWrapper 生成相机到各坐标系的变换矩阵
一、背景
在自动驾驶系统中，车辆配备了多种传感器，如相机、激光雷达（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: novatel
child_frame_id: velodyne64
transform:
translation:
x: 0.000000000
y: 0.414000000
z: 0.897000000
rotation:
qx: 0.000000000
qy: 0.000000000
qz: 0.707100000
qw: 0.707100000
ChannelName: /tf
MessageType: apollo.transform.TransformStampeds
FrameRatio: 193.20
transforms: [0]
header:
timestamp_sec: 1513807877.710000038
sequence_num: 0
frame_id: world
child_frame_id: localization
transform:
translation:
x: 587121.863165810
y: 4141199.892281929
z: -31.851399446
rotation:
qx: -0.019235064
qy: 0.041703123
qz: -0.787122706
qw: 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 time
tf_static_received = False
tf_received = False
links = set()
def tf_static_callback(msg):
global tf_static_received, links
tf_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, links
tf_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:
break
time.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
四、小结
通过以上步骤，我们使用 Apollo 的 TransformWrapper 工具生成了相机到世界坐标系、车辆坐标系和激光雷达坐标系的变换矩阵。这些矩阵可以用于将相机数据转换到其他坐标系，实现多传感器数据的融合和统一处理。
1、关键点：
- 设置参数文件：确保传感器参数文件正确配置。
- 启动静态变换：发布传感器之间的固定变换关系。
- 获取动态变换：通过记录数据或实时定位数据获取动态变换。
- 使用
TransformWrapper：调用 API 获取相机到其他坐标系的变换矩阵。
这些变换矩阵是自动驾驶系统中多传感器融合的基础，对于后续的感知、定位和决策模块至关重要。
注意：在实际应用中，需要根据车辆的具体配置和传感器安装位置调整参数文件。此外，变换矩阵的精度直接影响到融合效果，因此需要确保参数文件的准确性。