Camera python known to participate Lidar2 Lidar1 outer Lidar1 to external reference, the external reference Lidar2 to seek Camera

Derivation equation:

Camra outside Lidar1 parameters: 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 to Lidar1 external reference: 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 newly generated outer reference to 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

Reference
Python array type in the file does not exist a method according to scientific notation