def _load_calib(self):
# """Load and compute intrinsic and extrinsic calibration parameters.# """
# We'll build the calibration parameters as a dictionary, then
# convert it to a namedtuple to prevent it from being modified later
data = {}
# Load the calibration file
calib_filepath = os.path.join(self.sequence_path, 'calib.txt')
filedata = utils.read_calib_file(calib_filepath)
# Create 3x4 projection matrices
# 1x12 to 3x4
P_rect_00 = np.reshape(filedata['P0'], (3, 4))
P_rect_10 = np.reshape(filedata['P1'], (3, 4))
P_rect_20 = np.reshape(filedata['P2'], (3, 4))
P_rect_30 = np.reshape(filedata['P3'], (3, 4))
# P_rect_40 = np.reshape(filedate['Tr'], (3, 4))
data['P_rect_00'] = P_rect_00
data['P_rect_10'] = P_rect_10
data['P_rect_20'] = P_rect_20
data['P_rect_30'] = P_rect_30
# Compute the rectified extrinsics from cam0 to camN
T1 = np.eye(4)
T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0]
T2 = np.eye(4)
T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0]
T3 = np.eye(4)
T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0]
# Compute the velodyne to rectified camera coordinate transforms
# vstack
data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4))
# now data['T_cam0_velo'] is 4x4
data['T_cam0_velo'] = np.vstack([data['T_cam0_velo'], [0, 0, 0, 1]])
data['T_cam1_velo'] = T1.dot(data['T_cam0_velo'])
data['T_cam2_velo'] = T2.dot(data['T_cam0_velo'])
data['T_cam3_velo'] = T3.dot(data['T_cam0_velo'])
# Compute the camera intrinsics
data['K_cam0'] = P_rect_00[0:3, 0:3]
data['K_cam1'] = P_rect_10[0:3, 0:3]
data['K_cam2'] = P_rect_20[0:3, 0:3]
data['K_cam3'] = P_rect_30[0:3, 0:3]
# Compute the stereo baselines in meters by projecting the origin of
# each camera frame into the velodyne frame and computing the distances
# between them
p_cam = np.array([0, 0, 0, 1])
p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam)
p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam)
p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam)
p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam)
data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline
data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline
self.calib = namedtuple('CalibData', data.keys())(*data.values())
ori_velo_paths = glob.glob(os.path.join(src_velo_dir, "*.bin"))
# print(ori_velo_paths)
for ori_velo_path in tqdm.tqdm(sorted(ori_velo_paths)):
velo_idx = get_filename(ori_velo_path, False)
calib_path = get_file_path_by_idx(velo_idx, src_calib_dir)
label_path = get_file_path_by_idx(velo_idx, src_label_dir)
image_path = get_file_path_by_idx(velo_idx, src_image_dir)
output_velo_path = os.path.join(output_disturb_dir, velo_idx + ".bin")
output_viz_velo_ori_path = os.path.join(output_viz_original_dir,
velo_idx + ".jpg")
output_viz_velo_disturb_path = os.path.join(output_viz_disturb_dir,
velo_idx + ".jpg")
# Load calibration
calib = read_calib_file(calib_path)
# Load labels
labels = load_label(label_path)
# Load Lidar PC
pc_velo = load_velo_scan(ori_velo_path)[:, :3]
proj_cam_to_velo = project_cam2_to_velo(calib)
temp = np.asarray([[1, 1, 1]])
delete_inds = np.asarray([0])
for obj in labels:
# get obj range info
range_info = get_obj_range_info(obj)
inds = get_obj_inds(pc_velo, range_info)
selected = pc_velo[inds]
selected = selected[selected[:, 2].argsort()]
def load_camera_matrix(self, path, sample_image):
sample_height, sample_width = sample_image.shape[
0], sample_image.shape[1]
zoom_y = self.img_height / sample_height
zoom_x = self.img_width / sample_width
self.camera_matrix = read_calib_file(2, path, zoom_x, zoom_y)
cmap = np.array([cmap(i) for i in range(256)])[:, :3] * 255
for i in range(imgfov_points.shape[1]):
depth = imgfov_pc_cam2[2, i]
color = cmap[int(640.0 / depth) % 256, :]
cv2.circle(img, (int(np.round(
imgfov_points[0, i])), int(np.round(imgfov_points[1, i]))),
2,
color=tuple(color),
thickness=-1)
return img
def show(img):
plt.imshow(img)
plt.yticks([])
plt.xticks([])
plt.show()
if __name__ == '__main__':
# loading calibration
calib = read_calib_file('./calib.txt')
# loading image
img = load_image('./left_images/00000000.png')
# loading point cloud
point_cloud = load_point_cloud('./point_clouds/00000000.bin')
img = draw_lidar_on_image(calib, img, point_cloud)
show(img)