in tools/waymo/converter.py [0:0]
def save_calib(self, frame, file_idx, frame_idx):
""" parse and save the calibration data
:param frame: open dataset frame proto
:param file_idx: the current file number
:param frame_idx: the current frame number
:return:
"""
# kitti:
# bbox in reference camera frame (right-down-front)
# image_x_coord = Px * R0_rect * R0_rot * bbox_coord
# lidar points in lidar frame (front-right-up)
# image_x_coord = Px * R0_rect * Tr_velo_to_cam * lidar_coord
# note: R0_rot is caused by bbox rotation
# Tr_velo_to_cam projects lidar points to cam_0 frame
# waymo:
# bbox in vehicle frame, hence, use a virtual reference frame
# since waymo camera uses frame front-left-up, the virtual reference frame (right-down-front) is
# built on a transformed front camera frame, name this transform T_front_cam_to_ref
# and there is no rectified camera frame
# image_x_coord = intrinsics_x * Tr_front_cam_to_cam_x * inv(T_front_cam_to_ref) * R0_rot * bbox_coord(now in ref frame)
# lidar points in vehicle frame
# image_x_coord = intrinsics_x * Tr_front_cam_to_cam_x * inv(T_front_cam_to_ref) * T_front_cam_to_ref * Tr_velo_to_front_cam * lidar_coord
# hence, waymo -> kitti:
# set Tr_velo_to_cam = T_front_cam_to_ref * Tr_vehicle_to_front_cam = T_front_cam_to_ref * inv(Tr_front_cam_to_vehicle)
# as vehicle and lidar use the same frame after fusion
# set R0_rect = identity
# set P2 = front_cam_intrinsics * Tr_waymo_to_conv * Tr_front_cam_to_front_cam * inv(T_front_cam_to_ref)
# note: front cam is cam_0 in kitti, whereas has name = 1 in waymo
# note: waymo camera has a front-left-up frame,
# instead of the conventional right-down-front frame
# Tr_waymo_to_conv is used to offset this difference. However, Tr_waymo_to_conv is the same as
# T_front_cam_to_ref, hence,
# set P2 = front_cam_intrinsics
calib_context = ''
# front-left-up -> right-down-front
# T_front_cam_to_ref = np.array([
# [0.0, -1.0, 0.0],
# [-1.0, 0.0, 0.0],
# [0.0, 0.0, 1.0]
# ])
T_front_cam_to_ref = np.array([
[0.0, -1.0, 0.0],
[0.0, 0.0, -1.0],
[1.0, 0.0, 0.0]
])
# T_ref_to_front_cam = np.array([
# [0.0, 0.0, 1.0],
# [-1.0, 0.0, 0.0],
# [0.0, -1.0, 0.0]
# ])
# print('context\n',frame.context)
for camera in frame.context.camera_calibrations:
if camera.name == 1: # FRONT = 1, see dataset.proto for details
T_front_cam_to_vehicle = np.array(camera.extrinsic.transform).reshape(4, 4)
# print('T_front_cam_to_vehicle\n', T_front_cam_to_vehicle)
T_vehicle_to_front_cam = np.linalg.inv(T_front_cam_to_vehicle)
front_cam_intrinsic = np.zeros((3, 4))
front_cam_intrinsic[0, 0] = camera.intrinsic[0]
front_cam_intrinsic[1, 1] = camera.intrinsic[1]
front_cam_intrinsic[0, 2] = camera.intrinsic[2]
front_cam_intrinsic[1, 2] = camera.intrinsic[3]
front_cam_intrinsic[2, 2] = 1
break
# print('front_cam_intrinsic\n', front_cam_intrinsic)
self.T_front_cam_to_ref = T_front_cam_to_ref.copy()
self.T_vehicle_to_front_cam = T_vehicle_to_front_cam.copy()
identity_3x4 = np.eye(4)[:3, :]
# although waymo has 5 cameras, for compatibility, we produces 4 P
for i in range(4):
if i == 2:
# note: front camera is labeled camera 2 (kitti) or camera 0 (waymo)
# other Px are given dummy values. this is to ensure compatibility. They are seldom used anyway.
# tmp = cart_to_homo(np.linalg.inv(T_front_cam_to_ref))
# print(front_cam_intrinsic.shape, tmp.shape)
# P2 = np.matmul(front_cam_intrinsic, tmp).reshape(12)
P2 = front_cam_intrinsic.reshape(12)
calib_context += "P2: " + " ".join(['{}'.format(i) for i in P2]) + '\n'
else:
calib_context += "P" + str(i) + ": " + " ".join(['{}'.format(i) for i in identity_3x4.reshape(12)]) + '\n'
calib_context += "R0_rect" + ": " + " ".join(['{}'.format(i) for i in np.eye(3).astype(np.float32).flatten()]) + '\n'
Tr_velo_to_cam = self.cart_to_homo(T_front_cam_to_ref) @ np.linalg.inv(T_front_cam_to_vehicle)
# print('T_front_cam_to_vehicle\n', T_front_cam_to_vehicle)
# print('np.linalg.inv(T_front_cam_to_vehicle)\n', np.linalg.inv(T_front_cam_to_vehicle))
# print('cart_to_homo(T_front_cam_to_ref)\n', cart_to_homo(T_front_cam_to_ref))
# print('Tr_velo_to_cam\n',Tr_velo_to_cam)
calib_context += "Tr_velo_to_cam" + ": " + " ".join(['{}'.format(i) for i in Tr_velo_to_cam[:3, :].reshape(12)]) + '\n'
with open(self.calib_save_dir + '/' + str(file_idx).zfill(3) + str(frame_idx).zfill(3) + '.txt', 'w+') as fp_calib:
fp_calib.write(calib_context)