相机位姿转colmap位姿

def convert_extrinsic(camera_id, frame_id, input_extrinsic_filepath, frame_file_name, output_extrinsic_file):
with open(input_extrinsic_filepath) as iput_extrinsic:
contents = json.load(iput_extrinsic)
pos = np.asarray(contents['pos'])
rot = np.asarray(contents['rot'])

rotationInverse = R.from_euler('zyx', [rot[0], -rot[2], rot[1]], False).inv() #
pos = [-pos[1], -pos[2], pos[0]]
pos = -rotationInverse.as_matrix().dot(pos)
q = rotationInverse.as_quat()
w, x, y, z = q[3], q[0], q[1], q[2]

output_extrinsic_file.write(' '.join(
[str(frame_id), str(w), str(x), str(y), str(z), str(pos[0]), str(pos[1]), str(pos[2]), str(camera_id),
frame_file_name]))
output_extrinsic_file.write('\n') # fake, not used
output_extrinsic_file.write('\n')