公式推导:
Camra到Lidar1外参:camera_short_extrinsics.yaml
header:
seq: 0
stamp: 0.000000000
frame_id: velodyne32_2
child_frame_id: camera_short
transform:
translation:
x: 0.2686889257903541
y: -0.5643495276385981
z: 0.1665254108457194
rotation:
x: -0.5259060532077984
y: 0.4801982861606582
z: -0.4791352340064959
w: 0.5130904956255803
Lidar2到Lidar1外参:pandar40R_pandar40L_extrinsics.yaml
header:
stamp:
secs: 1422601952
nsecs: 288805456
seq: 0
frame_id: velodyne32
transform:
translation:
x: 0.134625
y: -2.20983
z: -0.100921
rotation:
x: -0.00324796
y: -0.00316435
z: 0.0266345
w: 0.999635
child_frame_id: pandar40R
# -*- coding: utf-8 -*-
import numpy as np
import yaml
from pyquaternion import Quaternion
path_camera = 'params/camera_short_extrinsics.yaml' #camera到lidar1的外参
path_other_lidar = 'params/pandar40R_pandar40L_extrinsics.yaml ' #lidar2到lidar1的外参
def load_yaml_file(file_path):
with open(file_path, 'r') as f:
ret = yaml.safe_load(f.read())
return ret
def transformer_rotation(extrinsics_file):
r = extrinsics_file['transform']['rotation']
rotation = Quaternion(np.array([r['w'], r['x'], r['y'], r['z']])).rotation_matrix #四元数直接转为旋转矩阵
return rotation
def transformer_translation(extrinsics_file):
t = extrinsics_file['transform']['translation']
translation = np.array([[t['x']], [t['y']], [t['z']]])
return translation
R1 = transformer_rotation(load_yaml_file(path_camera))
T1 = transformer_translation(load_yaml_file(path_camera))
R2 = transformer_rotation(load_yaml_file(path_other_lidar))
T2 = transformer_translation(load_yaml_file(path_other_lidar))
R3 = R2.T.dot(R1) #最终得出的旋转矩阵
T3 = R2.T.dot(T1 - T2) #最终得出的平移矩阵
#旋转矩阵转四元数
w = ((R3[0][0]+R3[1][1]+R3[2][2]+1)**0.5)/2 # **0.5 代表根号
x = (R3[2][1]-R3[1][2])/(4*w)
y = (R3[0][2]-R3[2][0])/(4*w)
z = (R3[1][0]-R3[0][1])/(4*w)
print(T3)
print(w, x, y, z)
#保存平移矩阵
np.savetxt("camera_pandar40R_extrinsics.yaml", T3, fmt=['%s']*T3.shape[1], newline='\n') #不按科学计数法存
#保存旋转矩阵
with open("camera_pandar40R_extrinsics.yaml", 'a') as f:
f.write('\n'+'x: '+str(x)+'\n'+'y: '+str(y)+'\n'+'z: '+str(z)+'\n'+'w: '+str(w))
#会在当前目录下生成camera到lidar2的外参: camera_pandar40R_extrinsics.yaml
新生成的Camera到Lidar2的外参: camera_pandar40R_extrinsics.yaml
0.22317165106091275 #x
1.6341933132694475 #y
0.27697171845416474 #z
x: -0.5127738867859182
y: 0.4972100456308417
z: -0.48940242025264474
w: 0.5003302735244916