def project_and_save(lidar_filepath, output_base_path, calib_data, cameras, db,
acc_sweeps, ip_basic):
log = lidar_filepath.parents[1].stem
lidar_timestamp = lidar_filepath.stem[3:]
neighbouring_timestamps = get_neighbouring_lidar_timestamps(
db, log, lidar_timestamp, acc_sweeps)
pts = load_ply(
lidar_filepath) # point cloud, numpy array Nx3 -> N 3D coords
points_h = point_cloud_to_homogeneous(pts).T
uv, uv_cam, valid_pts_bool = project_lidar_to_img_motion_compensated(
points_h, # these are recorded at lidar_time
copy.deepcopy(calib),
camera_name,
int(cam_timestamp),
int(lidar_timestamp),
str(split_dir),
log,
)
for camera_name in cameras:
uv, uv_cam, valid_pts_bool = project_lidar_to_img_motion_compensated(
points_h, calib_data, camera_name)
uv = uv[valid_pts_bool].astype(
np.int32) # Nx2 projected coords in pixels
uv_cam = uv_cam.T[valid_pts_bool] # Nx3 projected coords in meters
img_width, img_height = get_image_dims_for_camera(camera_name)
img = np.zeros((img_height, img_width))
img[uv[:, 1],
uv[:, 0]] = uv_cam[:, 2] # image of projected lidar measurements
lidar_filename = lidar_filepath.stem
output_dir = output_base_path / camera_name
output_dir.mkdir(parents=True, exist_ok=True)
output_path = output_dir / (lidar_filename + ".npz")
savez_compressed(str(output_path), img)
return None
def project_and_save(lidar_filepath, output_base_path, calib_data, cameras):
pts = load_ply(lidar_filepath) # point cloud, numpy array Nx3 -> N 3D coords
points_h = point_cloud_to_homogeneous(pts).T
for camera_name in cameras:
uv, uv_cam, valid_pts_bool = project_lidar_to_img(points_h, calib_data, camera_name)
uv = uv[valid_pts_bool].astype(np.int32) # Nx2 projected coords in pixels
uv_cam = uv_cam.T[valid_pts_bool] # Nx3 projected coords in meters
img_width, img_height = get_image_dims_for_camera(camera_name)
img = np.zeros((img_height, img_width))
img[uv[:, 1], uv[:, 0]] = uv_cam[:, 2] # image of projected lidar measurements
lidar_filename = lidar_filepath.stem
output_dir = output_base_path / camera_name
output_dir.mkdir(parents=True, exist_ok=True)
output_path = output_dir / (lidar_filename + ".npz")
savez_compressed(str(output_path), img)
return None
def draw_ground_pts_in_image(
sdb: SynchronizationDB,
lidar_points: np.ndarray,
city_to_egovehicle_se3: SE3,
dataset_map: ArgoverseMap,
log_id: str,
lidar_timestamp: int,
city_name: str,
dataset_dir: str,
experiment_prefix: str,
plot_ground: bool = True,
motion_compensate: bool = False,
camera: Optional[str] = None,
) -> Union[None, np.ndarray]:
"""Write an image to disk with rendered ground points for every camera.
Args:
sdb: instance of SynchronizationDB
lidar_points: Numpy array of shape (N,3) in egovehicle frame
city_to_egovehicle_se3: SE3 instance which takes a point in egovehicle frame and brings it into city frame
dataset_map: Map dataset instance
log_id: ID of the log
city_name: A city's name (e.g. 'MIA' or 'PIT')
motion_compensate: Whether to bring lidar points from world frame @ lidar timestamp, to world frame @ camera
timestamp
camera: camera name, if specified will return image of that specific camera, if None, will save all camera to
disk and return None
"""
# put into city coords, then prune away ground and non-RoI points
lidar_points = city_to_egovehicle_se3.transform_point_cloud(lidar_points)
lidar_points = dataset_map.remove_non_driveable_area_points(
lidar_points, city_name)
_, not_ground_logicals = dataset_map.remove_ground_surface(
copy.deepcopy(lidar_points), city_name, return_logicals=True)
lidar_points = lidar_points[np.logical_not(not_ground_logicals)
if plot_ground else not_ground_logicals]
# put back into ego-vehicle coords
lidar_points = city_to_egovehicle_se3.inverse_transform_point_cloud(
lidar_points)
calib_fpath = f"{dataset_dir}/{log_id}/vehicle_calibration_info.json"
calib_data = read_json_file(calib_fpath)
# repeat green to red colormap every 50 m.
colors_arr = np.array([
[color_obj.rgb]
for color_obj in Color("red").range_to(Color("green"), NUM_RANGE_BINS)
]).squeeze()
np.fliplr(colors_arr)
for cam_idx, camera_name in enumerate(RING_CAMERA_LIST +
STEREO_CAMERA_LIST):
im_dir = f"{dataset_dir}/{log_id}/{camera_name}"
# load images, e.g. 'image_raw_ring_front_center_000000486.jpg'
cam_timestamp = sdb.get_closest_cam_channel_timestamp(
lidar_timestamp, camera_name, log_id)
if cam_timestamp is None:
continue
im_fname = f"{camera_name}_{cam_timestamp}.jpg"
im_fpath = f"{im_dir}/{im_fname}"
# Swap channel order as OpenCV expects it -- BGR not RGB
# must make a copy to make memory contiguous
img = imageio.imread(im_fpath)[:, :, ::-1].copy()
points_h = point_cloud_to_homogeneous(copy.deepcopy(lidar_points)).T
if motion_compensate:
uv, uv_cam, valid_pts_bool = project_lidar_to_img_motion_compensated(
points_h, # these are recorded at lidar_time
copy.deepcopy(calib_data),
camera_name,
cam_timestamp,
lidar_timestamp,
dataset_dir,
log_id,
False,
)
else:
uv, uv_cam, valid_pts_bool = project_lidar_to_img(
points_h, copy.deepcopy(calib_data), camera_name, False)
if valid_pts_bool is None or uv is None or uv_cam is None:
continue
if valid_pts_bool.sum() == 0:
continue
uv = np.round(uv[valid_pts_bool]).astype(np.int32)
uv_cam = uv_cam.T[valid_pts_bool]
pt_ranges = np.linalg.norm(uv_cam[:, :3], axis=1)
rgb_bins = np.round(pt_ranges).astype(np.int32)
# account for moving past 100 meters, loop around again
rgb_bins = rgb_bins % NUM_RANGE_BINS
uv_colors = (255 * colors_arr[rgb_bins]).astype(np.int32)
img = draw_point_cloud_in_img_cv2(img, uv, np.fliplr(uv_colors))
if not Path(f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}"
).exists():
os.makedirs(
f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}")
save_dir = f"{experiment_prefix}_ground_viz/{log_id}/{camera_name}"
cv2.imwrite(f"{save_dir}/{camera_name}_{lidar_timestamp}.jpg", img)
if camera == camera_name:
return cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
return None
def dump_clipped_3d_cuboids_to_images(
log_ids: Sequence[str],
max_num_images_to_render: int,
data_dir: str,
experiment_prefix: str,
motion_compensate: bool = True,
) -> List[str]:
"""
We bring the 3D points into each camera coordinate system, and do the clipping there in 3D.
Args:
log_ids: A list of log IDs
max_num_images_to_render: maximum numbers of images to render.
data_dir: path to dataset with the latest data
experiment_prefix: Output directory
motion_compensate: Whether to motion compensate when projecting
Returns:
saved_img_fpaths
"""
saved_img_fpaths = []
dl = SimpleArgoverseTrackingDataLoader(data_dir=data_dir,
labels_dir=data_dir)
avm = ArgoverseMap()
for log_id in log_ids:
save_dir = f"{experiment_prefix}_{log_id}"
if not Path(save_dir).exists():
os.makedirs(save_dir)
city_name = dl.get_city_name(log_id)
log_calib_data = dl.get_log_calibration_data(log_id)
flag_done = False
for cam_idx, camera_name in enumerate(RING_CAMERA_LIST +
STEREO_CAMERA_LIST):
cam_im_fpaths = dl.get_ordered_log_cam_fpaths(log_id, camera_name)
for i, im_fpath in enumerate(cam_im_fpaths):
if i % 50 == 0:
logging.info("\tOn file %s of camera %s of %s", i,
camera_name, log_id)
cam_timestamp = Path(im_fpath).stem.split("_")[-1]
cam_timestamp = int(cam_timestamp)
# load PLY file path, e.g. 'PC_315978406032859416.ply'
ply_fpath = dl.get_closest_lidar_fpath(log_id, cam_timestamp)
if ply_fpath is None:
continue
lidar_pts = load_ply(ply_fpath)
save_img_fpath = f"{save_dir}/{camera_name}_{cam_timestamp}.jpg"
if Path(save_img_fpath).exists():
saved_img_fpaths += [save_img_fpath]
if max_num_images_to_render != -1 and len(
saved_img_fpaths) > max_num_images_to_render:
flag_done = True
break
continue
city_to_egovehicle_se3 = dl.get_city_to_egovehicle_se3(
log_id, cam_timestamp)
if city_to_egovehicle_se3 is None:
continue
lidar_timestamp = Path(ply_fpath).stem.split("_")[-1]
lidar_timestamp = int(lidar_timestamp)
labels = dl.get_labels_at_lidar_timestamp(
log_id, lidar_timestamp)
if labels is None:
logging.info("\tLabels missing at t=%s", lidar_timestamp)
continue
# Swap channel order as OpenCV expects it -- BGR not RGB
# must make a copy to make memory contiguous
img = imageio.imread(im_fpath)[:, :, ::-1].copy()
camera_config = get_calibration_config(log_calib_data,
camera_name)
planes = generate_frustum_planes(
camera_config.intrinsic.copy(), camera_name)
img = plot_lane_centerlines_in_img(
lidar_pts,
city_to_egovehicle_se3,
img,
city_name,
avm,
camera_config,
planes,
)
for label_idx, label in enumerate(labels):
obj_rec = json_label_dict_to_obj_record(label)
if obj_rec.occlusion == 100:
continue
cuboid_vertices = obj_rec.as_3d_bbox()
points_h = point_cloud_to_homogeneous(cuboid_vertices).T
if motion_compensate:
(
uv,
uv_cam,
valid_pts_bool,
K,
) = project_lidar_to_img_motion_compensated(
points_h, # these are recorded at lidar_time
copy.deepcopy(log_calib_data),
camera_name,
cam_timestamp,
lidar_timestamp,
data_dir,
log_id,
return_K=True,
)
else:
# project_lidar_to_img
(
uv,
uv_cam,
valid_pts_bool,
camera_config,
) = project_lidar_to_undistorted_img(
points_h, copy.deepcopy(log_calib_data),
camera_name)
if valid_pts_bool.sum() == 0:
continue
img = obj_rec.render_clip_frustum_cv2(
img,
uv_cam.T[:, :3],
planes.copy(),
copy.deepcopy(camera_config),
)
cv2.imwrite(save_img_fpath, img)
saved_img_fpaths += [save_img_fpath]
if max_num_images_to_render != -1 and len(
saved_img_fpaths) > max_num_images_to_render:
flag_done = True
break
if flag_done:
break
category_subdir = "amodal_labels"
if not Path(f"{experiment_prefix}_{category_subdir}").exists():
os.makedirs(f"{experiment_prefix}_{category_subdir}")
for cam_idx, camera_name in enumerate(RING_CAMERA_LIST +
STEREO_CAMERA_LIST):
# Write the cuboid video -- could also write w/ fps=20,30,40
if "stereo" in camera_name:
fps = 5
else:
fps = 30
img_wildcard = f"{save_dir}/{camera_name}_%*.jpg"
output_fpath = f"{experiment_prefix}_{category_subdir}/{log_id}_{camera_name}_{fps}fps.mp4"
write_nonsequential_idx_video(img_wildcard, output_fpath, fps)
return saved_img_fpaths
def project_and_save(argoverse_tracking_root_dir, camera_list, output_base_dir,
sample_paths):
relative_lidar_path = Path(sample_paths[0])
split = relative_lidar_path.parents[2].stem
log = relative_lidar_path.parents[1].stem
lidar_timestamp = int(relative_lidar_path.stem[3:])
lidar_filepath = argoverse_tracking_root_dir / relative_lidar_path
split_dir = argoverse_tracking_root_dir / split
log_dir = split_dir / log
with open(str(log_dir / "vehicle_calibration_info.json"), "r") as f:
calib_data = json.load(f)
pc = load_ply_ring(str(lidar_filepath))
tf_down_lidar_rot = Rotation.from_quat(
calib_data['vehicle_SE3_down_lidar_']['rotation']['coefficients'])
tf_down_lidar_tr = calib_data['vehicle_SE3_down_lidar_']['translation']
tf_down_lidar = np.eye(4)
tf_down_lidar[0:3, 0:3] = tf_down_lidar_rot.as_matrix()
tf_down_lidar[0:3, 3] = tf_down_lidar_tr
tf_up_lidar_rot = Rotation.from_quat(
calib_data['vehicle_SE3_up_lidar_']['rotation']['coefficients'])
tf_up_lidar_tr = calib_data['vehicle_SE3_up_lidar_']['translation']
tf_up_lidar = np.eye(4)
tf_up_lidar[0:3, 0:3] = tf_up_lidar_rot.as_matrix()
tf_up_lidar[0:3, 3] = tf_up_lidar_tr
pc_up, pc_down = separate_pc(pc, tf_up_lidar, tf_down_lidar)
beams = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
mask = np.logical_or.reduce([pc_up[:, 4] == beam for beam in beams])
pts = pc_up[mask][:, :3]
points_h = point_cloud_to_homogeneous(pts).T
for cam_idx, camera_name in enumerate(camera_list):
img_rel_path = sample_paths[1 + cam_idx][0]
closest_cam_timestamp = int(
Path(img_rel_path).stem[len(camera_name) + 1:])
uv, uv_cam, valid_pts_bool = project_lidar_to_img_motion_compensated(
points_h, # these are recorded at lidar_time
copy.deepcopy(calib_data),
camera_name,
closest_cam_timestamp,
lidar_timestamp,
str(split_dir),
log,
)
img_width, img_height = get_image_dims_for_camera(camera_name)
img = np.zeros((img_height, img_width))
if valid_pts_bool is None or uv is None:
print(
f"uv or valid_pts_bool is None: camera {camera_name} ts {closest_cam_timestamp}, {lidar_filepath}"
)
else:
uv = uv[valid_pts_bool].astype(
np.int32) # Nx2 projected coords in pixels
uv_cam = uv_cam.T[valid_pts_bool] # Nx3 projected coords in meters
img[uv[:, 1],
uv[:, 0]] = uv_cam[:,
2] # image of projected lidar measurements
lidar_filename = lidar_filepath.stem
output_dir = output_base_dir / split / log / camera_name
output_dir.mkdir(parents=True, exist_ok=True)
output_path = output_dir / (lidar_filename + ".npz")
savez_compressed(str(output_path), img)
return None
