如何实现xtreme1与Apollo相机外参的双向转换

一、概述

在自动驾驶和计算机视觉领域，多传感器融合是一项关键技术。为了实现激光雷达和相机的数据融合，我们需要知道这两个传感器之间的精确空间关系，这个关系就是相机外参。

xtreme1和Apollo是两个不同的自动驾驶平台，它们使用不同的格式来描述相机外参。本文将详细介绍这两种格式的差异，并展示如何在这两种格式之间进行双向转换。

二、什么是相机外参？

简单来说，相机外参描述了相机在三维空间中的位置和朝向。在自动驾驶系统中，通常以激光雷达为基准坐标系，相机外参就是描述从激光雷达到相机的坐标系变换关系。

想象一下：激光雷达告诉我们某个物体在它的坐标系中的位置，而相机也看到了这个物体。为了让这两个"观测"能够对应起来，我们需要知道如何将一个坐标系中的点转换到另一个坐标系中。

三、两种格式的主要区别

1、xtreme1格式

• 使用4×4变换矩阵（行主序或列主序）
• 表示从激光雷达到相机的变换
• 格式：一个包含16个元素的数组，可以重塑为4×4矩阵

2、Apollo格式

• 使用四元数 + 平移向量
• 表示从相机到激光雷达的变换（注意方向！）
• 格式：YAML格式，包含四元数(w,x,y,z)和平移向量(x,y,z)

3、关键差异

1. 方向相反：xtreme1是"激光雷达→相机"，Apollo是"相机→激光雷达"
2. 表示形式不同：xtreme1用矩阵，Apollo用四元数+平移向量

四、转换原理

1. 核心数学概念

1.1、变换矩阵

一个4×4变换矩阵可以表示为：

[ R t ] [ 0 1 ]

其中R是3×3旋转矩阵，t是3维平移向量。

1.2、四元数

四元数是一种表示三维旋转的数学工具，比旋转矩阵更紧凑（4个值 vs 9个值），且没有奇异性问题。

2. 转换流程

xtreme1矩阵（激光雷达→相机） ↓ 求逆矩阵 相机→激光雷达变换矩阵 ↓ 提取R和t 旋转矩阵 + 平移向量 ↓ 旋转矩阵转四元数 四元数 + 平移向量（Apollo格式）

五、完整实现

下面我们通过详细的代码示例来演示如何进行双向转换。

''' 代码功能：演示xtreme1中camera_external与Apollo相机外参之间的转换关系 核心原理：两种格式本质上都是描述从激光雷达到相机的坐标变换，只是表示形式不同 转换流程：旋转矩阵 ↔ 四元数 ↔ 变换矩阵 '''""" 将3x3旋转矩阵转换为四元数 原理：通过矩阵的迹和反对称元素计算四元数的四个分量 参数： R: 3x3旋转矩阵 返回： [qw, qx, qy, qz] 四元数，其中qw是标量部分 """# 确保输入是numpy数组格式importjsonimportyamlimportnumpyasnpfrompyquaternionimportQuaterniondefmatrix_to_quaternion(R):""" 将3x3旋转矩阵转换为四元数 原理：通过矩阵的迹和反对称元素计算四元数的四个分量 参数： R: 3x3旋转矩阵 返回： [qw, qx, qy, qz] 四元数，其中qw是标量部分 """# 确保输入是numpy数组格式R=np.array(R)# 计算四元数的四个分量，使用max(0, ...)避免负数开方# 这些公式来源于旋转矩阵到四元数的标准转换公式qw=np.sqrt(max(0,1+R[0,0]+R[1,1]+R[2,2]))/2qx=np.sqrt(max(0,1+R[0,0]-R[1,1]-R[2,2]))/2qy=np.sqrt(max(0,1-R[0,0]+R[1,1]-R[2,2]))/2qz=np.sqrt(max(0,1-R[0,0]-R[1,1]+R[2,2]))/2# 根据旋转矩阵的反对称元素确定符号，确保四元数方向正确qx=np.copysign(qx,R[2,1]-R[1,2])qy=np.copysign(qy,R[0,2]-R[2,0])qz=np.copysign(qz,R[1,0]-R[0,1])returnnp.array([qw,qx,qy,qz])# xtreme1格式的相机参数配置# 特点：使用4x4变换矩阵（行主序或列主序），表示从激光雷达到相机的变换xtreme1_camera_config=''' { "camera_internal": { "fx": 569.6122896303689, "fy": 576.6583816595539, "cx": 787.6247097810974, "cy": 362.8023638439239 }, "width": 1600, "height": 900, "camera_external": [ -0.006547572308123936, -0.22063892941095503, 0.975333579922919, 0.0, -0.9983898066288098, 0.056401333632504734, 0.0060566974633405575, 0.0, -0.05634645788829527, -0.9737234475932385, -0.22065294987962453, 0.0, -0.055100120607299734, 0.035365098288374454, -1.154071016217558, 1.0 ], "rowMajor": false } '''# Apollo格式的相机外参# 特点：使用四元数表示旋转 + 平移向量，表示从相机到激光雷达的变换apollo_camera_front_extrinsics=''' child_frame_id: camera_front header: frame_id: lidar128_center transform: rotation: x: -0.53798328156856234 y: 0.56648077129426289 z: -0.42705189657045695 w: 0.45530232049620079 translation: x: 1.13304636113375 y: -0.050016178469415369 z: -0.22331804529458971 '''# ==================== 第一部分：处理xtreme1数据 ====================# 1. 解析xtreme1配置并获取相机外参矩阵camera_config=json.loads(xtreme1_camera_config)# 将一维数组按列主序('F')重塑为4x4变换矩阵# 这个矩阵表示从激光雷达坐标系到相机坐标系的变换: P_camera = T_lidar2cam * P_lidarxtreme1_lidar2cam_rt=np.array(camera_config["camera_external"],dtype=np.float32).reshape((4,4),order='F')# order='F'表示列主序（Fortran风格）print("xtreme1相机外参(4x4变换矩阵，激光雷达到相机)")print(xtreme1_lidar2cam_rt)# 2. 求逆矩阵，得到从相机到激光雷达的变换# 因为Apollo格式是从相机到激光雷达，所以需要求逆# P_lidar = T_cam2lidar * P_cameracam2lidar_rt=np.linalg.inv(xtreme1_lidar2cam_rt)# 提取旋转矩阵R和平移向量tR=cam2lidar_rt[:3,:3]# 3x3旋转矩阵t=cam2lidar_rt[:3,3]# 3维平移向量# 将旋转矩阵转换为四元数表示xtreme1_r=matrix_to_quaternion(R)xtreme1_t=tprint("\nxtreme1相机外参(转换为Apollo格式：四元数+平移)")print("rotation(w,x,y,z):",xtreme1_r)# 注意：这里是[w, x, y, z]顺序print("translation(x,y,z):",xtreme1_t)# ==================== 第二部分：处理Apollo数据 ====================# 3. 解析Apollo格式的外参配置config=yaml.safe_load(apollo_camera_front_extrinsics)extrinsic=config['transform']# 提取平移向量translation=extrinsic['translation']# 提取旋转四元数（注意Apollo格式中w是最后一个）rotation=extrinsic['rotation']# 转换为标准四元数格式：[w, x, y, z]apollo_r=np.array([rotation['w'],# 标量部分rotation['x'],# 向量部分rotation['y'],rotation['z']])# 转换为平移向量apollo_t=np.array([translation['x'],translation['y'],translation['z']])print("\nApollo相机外参(原始格式)")print("rotation(w,x,y,z):",apollo_r)print("translation(x,y,z):",apollo_t)# ==================== 第三部分：比较两种格式的差异 ====================print("\n比较xtreme1和Apollo格式是否等效：")print("旋转四元数是否一致:",np.allclose(apollo_r,xtreme1_r,atol=1e-6))print("平移向量是否一致:",np.allclose(apollo_t,xtreme1_t,atol=1e-6))# ==================== 第四部分：反向转换验证 ====================# 5. 将Apollo格式（四元数+平移）转换回xtreme1格式（4x4矩阵）# 使用pyquaternion库将四元数转换为旋转矩阵cam2lidar_rt=np.eye(4)# 创建单位矩阵cam2lidar_rt[:3,:3]=Quaternion(apollo_r).rotation_matrix# 设置旋转部分cam2lidar_rt[:3,3]=apollo_t# 设置平移部分# 求逆得到激光雷达到相机的变换矩阵lidar2cam_rt=np.linalg.inv(cam2lidar_rt)# 按列主序展开为一维数组（xtreme1格式）camera_external=lidar2cam_rt.flatten('F').tolist()# 重新reshape为4x4矩阵验证apollo_lidar2cam_rt=np.array(camera_external,dtype=np.float32).reshape((4,4),order='F')print("\nApollo相机外参(转换为xtreme1格式：4x4变换矩阵)")print(apollo_lidar2cam_rt)# 6. 最终验证：比较两个变换矩阵是否一致print("\n最终验证：两个格式的变换矩阵是否完全等效")print("矩阵是否一致:",np.allclose(apollo_lidar2cam_rt,xtreme1_lidar2cam_rt,atol=1e-6))''' 关键点总结： 1. xtreme1使用4x4变换矩阵，表示从激光雷达到相机的变换 2. Apollo使用四元数+平移向量，表示从相机到激光雷达的变换 3. 两者互为逆变换，需要进行矩阵求逆操作 4. 四元数与旋转矩阵可以相互转换 5. 列主序('F')与行主序('C')在reshape时需要注意 6. 比较浮点数时应使用np.allclose而非==，考虑数值误差 '''

六、代码输出

xtreme1相机外参(4x4变换矩阵，激光雷达到相机)[[-0.00654757 -0.9983898 -0.05634646 -0.05510012][-0.220638930.05640133-0.97372350.0353651][0.97533360.0060567-0.22065295 -1.154071][0.0.0.1.]]xtreme1相机外参(转换为Apollo格式：四元数+平移)rotation(w,x,y,z):[0.45530232-0.537983280.56648077-0.4270519]translation(x,y,z):[1.1330463-0.05001618 -0.22331804]Apollo相机外参(原始格式)rotation(w,x,y,z):[0.45530232-0.537983280.56648077-0.4270519]translation(x,y,z):[1.13304636-0.05001618 -0.22331805]比较xtreme1和Apollo格式是否等效： 旋转四元数是否一致: True 平移向量是否一致: True Apollo相机外参(转换为xtreme1格式：4x4变换矩阵)[[-0.00654757 -0.9983898 -0.05634646 -0.05510012][-0.220638930.05640133-0.97372350.0353651][0.97533360.0060567-0.22065295 -1.154071][0.0.0.1.]]最终验证：两个格式的变换矩阵是否完全等效 矩阵是否一致: True