如何实现xtreme1与Apollo相机外参的双向转换
一、概述
在自动驾驶和计算机视觉领域，多传感器融合是一项关键技术。为了实现激光雷达和相机的数据融合，我们需要知道这两个传感器之间的精确空间关系，这个关系就是相机外参。
xtreme1和Apollo是两个不同的自动驾驶平台，它们使用不同的格式来描述相机外参。本文将详细介绍这两种格式的差异，并展示如何在这两种格式之间进行双向转换。
二、什么是相机外参？
简单来说，相机外参描述了相机在三维空间中的位置和朝向。在自动驾驶系统中，通常以激光雷达为基准坐标系，相机外参就是描述从激光雷达到相机的坐标系变换关系。
想象一下：激光雷达告诉我们某个物体在它的坐标系中的位置，而相机也看到了这个物体。为了让这两个"观测"能够对应起来，我们需要知道如何将一个坐标系中的点转换到另一个坐标系中。
三、两种格式的主要区别
1、xtreme1格式
- 使用4×4变换矩阵（行主序或列主序）
- 表示从激光雷达到相机的变换
- 格式：一个包含16个元素的数组，可以重塑为4×4矩阵
2、Apollo格式
- 使用四元数 + 平移向量
- 表示从相机到激光雷达的变换（注意方向！）
- 格式：YAML格式，包含四元数(w,x,y,z)和平移向量(x,y,z)
3、关键差异
- 方向相反：xtreme1是"激光雷达→相机"，Apollo是"相机→激光雷达"
- 表示形式不同：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格式）五、完整实现
下面我们通过详细的代码示例来演示如何进行双向转换。
python
'''
代码功能：演示xtreme1中camera_external与Apollo相机外参之间的转换关系
核心原理：两种格式本质上都是描述从激光雷达到相机的坐标变换，只是表示形式不同
转换流程：旋转矩阵 ↔ 四元数 ↔ 变换矩阵
''' """
将3x3旋转矩阵转换为四元数
原理：通过矩阵的迹和反对称元素计算四元数的四个分量
参数：
R: 3x3旋转矩阵
返回：
[qw, qx, qy, qz] 四元数，其中qw是标量部分
"""
# 确保输入是numpy数组格式
import json
import yaml
import numpy as np
from pyquaternion import Quaternion
def matrix_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])) / 2
qx = np.sqrt(max(0, 1 + R[0,0] - R[1,1] - R[2,2])) / 2
qy = np.sqrt(max(0, 1 - R[0,0] + R[1,1] - R[2,2])) / 2
qz = 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])
return np.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_lidar
xtreme1_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_camera
cam2lidar_rt=np.linalg.inv(xtreme1_lidar2cam_rt)
# 提取旋转矩阵R和平移向量t
R = cam2lidar_rt[:3, :3] # 3x3旋转矩阵
t = cam2lidar_rt[:3, 3] # 3维平移向量
# 将旋转矩阵转换为四元数表示
xtreme1_r = matrix_to_quaternion(R)
xtreme1_t = t
print("\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而非==，考虑数值误差
'''六、代码输出
bash
xtreme1相机外参(4x4变换矩阵，激光雷达到相机)
[[-0.00654757 -0.9983898 -0.05634646 -0.05510012]
[-0.22063893 0.05640133 -0.9737235 0.0353651 ]
[ 0.9753336 0.0060567 -0.22065295 -1.154071 ]
[ 0. 0. 0. 1. ]]
xtreme1相机外参(转换为Apollo格式：四元数+平移)
rotation(w,x,y,z): [ 0.45530232 -0.53798328 0.56648077 -0.4270519 ]
translation(x,y,z): [ 1.1330463 -0.05001618 -0.22331804]
Apollo相机外参(原始格式)
rotation(w,x,y,z): [ 0.45530232 -0.53798328 0.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.22063893 0.05640133 -0.9737235 0.0353651 ]
[ 0.9753336 0.0060567 -0.22065295 -1.154071 ]
[ 0. 0. 0. 1. ]]
最终验证：两个格式的变换矩阵是否完全等效
矩阵是否一致: True