def get_calibration_config(calibration: Dict[str, Any],
camera_name: str) -> CameraConfig:
"""
Get calibration config dumped with log.
Args:
calibration
camera_name: name of the camera.
Returns:
instance of CameraConfig class
"""
all_camera_data = calibration["camera_data_"]
for camera_data in all_camera_data:
if camera_name in camera_data["key"]:
camera_calibration = camera_data["value"]
break
else:
raise ValueError(f"Unknown camera name: {camera_name}")
camera_extrinsic_matrix = get_camera_extrinsic_matrix(camera_calibration)
camera_intrinsic_matrix = get_camera_intrinsic_matrix(camera_calibration)
img_width, img_height = get_image_dims_for_camera(camera_name)
if img_width is None or img_height is None:
raise ValueError(
f"Specified camera has unknown dimensions: {camera_name}")
return CameraConfig(
camera_extrinsic_matrix,
camera_intrinsic_matrix,
img_width,
img_height,
camera_calibration["distortion_coefficients_"],
)
def generate_frustum_planes(
K: np.ndarray,
camera_name: str,
near_clip_dist: float = 0.5) -> Optional[List[np.ndarray]]:
"""Compute the planes enclosing the field of view (viewing frustum) for a single camera.
We do this using similar triangles.
tan(theta/2) = (0.5 * height)/focal_length
"theta" is the vertical FOV. Similar for horizontal FOV.
height and focal_length are both in pixels.
Note that ring cameras and stereo cameras have different image widths
and heights, affecting the field of view.
Ring Camera intrinsics K look like (in pixels)::
[1400, 0, 964] [fx,skew,cx]
[ 0,1403, 605] for [-, fy,cy]
[ 0, 0, 1] [0, 0, 1]
Args:
K: Array of shape (3, 3) representing camera intrinsics matrix
camera_name: String representing the camera name to get the dimensions of and compute the FOV for
near_clip_dist: The distance for the near clipping plane in meters
Returns:
planes: List of length 5, where each list element is an Array of shape (4,)
representing the equation of a plane, e.g. (a, b, c, d) in ax + by + cz = d
"""
img_width, img_height = get_image_dims_for_camera(camera_name)
if img_width is None or img_height is None:
return None
# frustum starts at optical center [0,0,0]
fx = K[0, 0]
fy = K[1, 1]
right_plane = form_right_clipping_plane(fx, img_width)
left_plane = form_left_clipping_plane(fx, img_width)
near_plane = form_near_clipping_plane(near_clip_dist)
# The horizontal and vertical focal lengths should be very close to equal,
# otherwise something went wrong when forming K matrix.
assert np.absolute(fx - fy) < 10
low_plane = form_low_clipping_plane(fx, img_height)
top_plane = form_top_clipping_plane(fx, img_height)
planes = [left_plane, right_plane, near_plane, low_plane, top_plane]
return planes
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 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
