def plot_argoverse_trajectory(ts1: int, ts2: int, dataset_dir: str, log_id: str):
""" """
city_SE3_egot1 = get_city_SE3_egovehicle_at_sensor_t(ts1, dataset_dir, log_id)
city_SE3_egot2 = get_city_SE3_egovehicle_at_sensor_t(ts2, dataset_dir, log_id)
# plot the vehicle movement
sensor_folder_wildcard = f"{dataset_dir}/{log_id}/poses/city_SE3_egovehicle*.json"
lidar_timestamps = get_timestamps_from_sensor_folder(sensor_folder_wildcard)
colors_arr = np.array(
[ [color_obj.rgb] for color_obj in Color("red").range_to(Color("green"), len(lidar_timestamps) // 49 ) ]
).squeeze()
plt.close('all')
for k,ts in enumerate(lidar_timestamps[::50]):
city_SE3_egov_t = get_city_SE3_egovehicle_at_sensor_t(ts, dataset_dir, log_id)
t = city_SE3_egov_t.translation
plt.scatter(t[0], t[1], 20, marker='.', color=colors_arr[k])
t1 = city_SE3_egot1.translation
t2 = city_SE3_egot2.translation
print('t1 =', t1)
print('t2 =', t2)
plt.scatter(t1[0], t1[1], 10, marker='o', color='m')
plt.scatter(t2[0], t2[1], 10, marker='o', color='c')
posx_1 = city_SE3_egot1.transform_point_cloud(np.array([[3,0,0]])).squeeze()
posy_1 = city_SE3_egot1.transform_point_cloud(np.array([[0,3,0]])).squeeze()
posx_2 = city_SE3_egot2.transform_point_cloud(np.array([[3,0,0]])).squeeze()
posy_2 = city_SE3_egot2.transform_point_cloud(np.array([[0,3,0]])).squeeze()
plt.plot([t1[0], posx_1[0]], [t1[1], posx_1[1]], 'b')
plt.plot([t1[0], posy_1[0]], [t1[1], posy_1[1]], 'k')
plt.plot([t2[0], posx_2[0]], [t2[1], posx_2[1]], 'b')
plt.plot([t2[0], posy_2[0]], [t2[1], posy_2[1]], 'k')
plt.axis('equal')
plt.title('Egovehicle trajectory')
plt.xlabel('x city coordinate')
plt.ylabel('y city coordinate')
plt.show()
egot1_SE3_city = city_SE3_egot1.inverse()
egot1_SE3_egot2 = egot1_SE3_city.right_multiply_with_se3(city_SE3_egot2)
# rotates i1's frame to i2's frame
# 1R2 bring points in 2's frame into 1's frame
# 1R2 is the relative rotation from 1's frame to 2's frame
egot1_R_egot2 = egot1_SE3_egot2.rotation
egot1_t_egot2 = egot1_SE3_egot2.translation
r = Rotation.from_matrix(egot1_R_egot2)
print('egot1_R_egot2 from ground truth: ', r.as_euler('zyx', degrees=True))
print('egot1_t_egot2 = ', np.round(egot1_t_egot2,2))
def plot_argoverse_epilines_from_ground_truth_poses(ts1: int, ts2: int, img1, img2, K):
""" """
city_SE3_egot1 = get_city_SE3_egovehicle_at_sensor_t(ts1, dataset_dir, log_id)
city_SE3_egot2 = get_city_SE3_egovehicle_at_sensor_t(ts2, dataset_dir, log_id)
camera_T_egovehicle = calib_dict['ring_front_center'].extrinsic
camera_T_egovehicle = SE3(rotation=camera_T_egovehicle[:3,:3], translation=camera_T_egovehicle[:3,3])
egovehicle_T_camera = camera_T_egovehicle.inverse()
city_SE3_cam1 = city_SE3_egot1.compose(egovehicle_T_camera)
city_SE3_cam2 = city_SE3_egot2.compose(egovehicle_T_camera)
cam1_SE3_city = city_SE3_cam1.inverse()
cam1_SE3_cam2 = cam1_SE3_city.compose(city_SE3_cam2)
# rotates i1's frame to i2's frame
# 1R2 bring points in 2's frame into 1's frame
# 1R2 is the relative rotation from 1's frame to 2's frame
cam1_R_cam2 = cam1_SE3_cam2.rotation
cam1_t_cam2 = cam1_SE3_cam2.translation
r = Rotation.from_matrix(cam1_R_cam2)
print('cam1_R_cam2 =', r.as_euler('zyx', degrees=True))
print('Recover t=', cam1_t_cam2, ' up to scale ', cam1_t_cam2 / np.linalg.norm(cam1_t_cam2))
cam2_SE3_cam1 = cam1_SE3_cam2.inverse()
cam2_R_cam1 = cam2_SE3_cam1.rotation
cam2_t_cam1 = cam2_SE3_cam1.translation
# use correct ground truth relationship to generate gt_E
# and then generate correspondences using
cam2_E_cam1 = compute_essential_matrix(cam2_R_cam1, cam2_t_cam1)
cam2_F_cam1 = get_fmat_from_emat(cam2_E_cam1, K1=K, K2=K)
img_h, img_w, _ = img1.shape
pkl_fpath = f'/Users/johnlambert/Downloads/visual-odometry-tutorial/labeled_correspondences/argoverse_1_E_0.pkl'
corr_data = load_pkl_correspondences(pkl_fpath)
pts_left = np.hstack([corr_data.X1.reshape(-1,1), corr_data.Y1.reshape(-1,1) ]).astype(np.int32)
pts_right = np.hstack([corr_data.X2.reshape(-1,1), corr_data.Y2.reshape(-1,1)]).astype(np.int32)
pdb.set_trace()
draw_epilines(pts_left, pts_right, img1, img2, cam2_F_cam1)
plt.show()
draw_epipolar_lines(cam2_F_cam1, img1, img2, pts_left, pts_right)
pts_left = cartesian_to_homogeneous(pts_left)
pts_right = cartesian_to_homogeneous(pts_right)
for (pt1, pt2) in zip(pts_left, pts_right):
epi_error = pt2.dot(cam2_F_cam1).dot(pt1)
print('Error: ', epi_error)
def convert_bbox_to_city_frame(
self,
lidar_timestamp_ns: int,
dataset_dir: str,
log_id: str,
bbox_ego_frame: np.ndarray,
) -> Tuple[np.ndarray, Dict[str, np.ndarray]]:
"""Convert bounding box to city frame.
Args:
lidar_timestamp_ns (int): Lidar timestamp.
dataset_dir (str): representing full path to the log_ids.
log_id (str): e.g. '3ced8dba-62d0-3930-8f60-ebeea2feabb8'.
bbox_ego_frame (np.ndarray): Numpy array of shape (4,3), representing bounding box in egovehicle frame
Returned:
bbox_city_fr: Numpy array of shape (4,3), representing bounding box in CITY frame
pose_city_to_ego: dictionary, has two fields: 'translation' and 'rotation'
describing the SE(3) for p_city = city_to_egovehicle_se3 * p_egovehicle
"""
city_to_egovehicle_se3 = get_city_SE3_egovehicle_at_sensor_t(
lidar_timestamp_ns, dataset_dir, log_id)
if city_to_egovehicle_se3 is None:
raise RuntimeError(
f"Could not get city to egovehicle coordinate transformation at timestamp {lidar_timestamp_ns}"
)
bbox_city_fr = city_to_egovehicle_se3.transform_point_cloud(
bbox_ego_frame)
pose_city_to_ego = {
"rotation": city_to_egovehicle_se3.rotation,
"translation": city_to_egovehicle_se3.translation,
}
return bbox_city_fr, pose_city_to_ego
def get_city_SE3_egovehicle(self, log_id: str,
timestamp: int) -> Optional[SE3]:
"""
Args:
log_id: str, unique ID of vehicle log
timestamp: int, timestamp of sensor observation, in nanoseconds
Returns:
city_SE3_egovehicle: SE3 transformation to bring points in egovehicle frame into city frame.
"""
return get_city_SE3_egovehicle_at_sensor_t(timestamp, self.data_dir,
log_id)
def get_pose(self, idx: int, log_id: Optional[str] = None) -> Optional[SE3]:
"""Get pose corresponding to an index in a particular log_id.
Args:
idx: Lidar frame index
log_id: ID of log to search (default: current log)
Returns:
Pose for a particular index
"""
if log_id is None:
log_id = self.current_log
self._ensure_lidar_timestamp_list_populated()
assert self._lidar_timestamp_list is not None
timestamp = self._lidar_timestamp_list[log_id][idx]
return get_city_SE3_egovehicle_at_sensor_t(timestamp, self.root_dir, log_id)
def group_and_SE3(cloud_dict, dataset=DATASET_DIR, log_id=LOG_ID):
timestamps = []
for key in sorted(cloud_dict.keys()):
timestamps.append(key)
if len(timestamps) == 5:
t0_to_map_SE3 = get_city_SE3_egovehicle_at_sensor_t(
str(timestamps[0]), dataset, log_id)
map_to_t0_SE3 = t0_to_map_SE3.inverse()
rotation = np.eye(3)
translation = np.array([-72, -40, 0])
t0_to_occ_SE3 = se3.SE3(rotation, translation)
for ts in timestamps:
pc = cloud_dict[ts]
pc = map_to_t0_SE3.transform_point_cloud(pc)
pc = t0_to_occ_SE3.transform_point_cloud(pc)
cloud_dict[ts] = pc
timestamps = []
for ts in timestamps:
del cloud_dict[ts]
return cloud_dict
def test_filter_objs_to_roi():
""" Use the map to filter out an object that lies outside the ROI in a parking lot """
avm = ArgoverseMap()
# should be outside of ROI
outside_obj = {
"center": {"x": -14.102872067388489, "y": 19.466695178746022, "z": 0.11740010190455852},
"rotation": {"x": 0.0, "y": 0.0, "z": -0.038991328555453404, "w": 0.9992395490058831},
"length": 4.56126567460171,
"width": 1.9370055686754908,
"height": 1.5820081349372281,
"track_label_uuid": "03a321bf955a4d7781682913884abf06",
"timestamp": 315970611820366000,
"label_class": "VEHICLE",
}
# should be inside the ROI
inside_obj = {
"center": {"x": -20.727430239506702, "y": 3.4488006757501353, "z": 0.4036619561689685},
"rotation": {"x": 0.0, "y": 0.0, "z": 0.0013102003738908123, "w": 0.9999991416871218},
"length": 4.507580779458834,
"width": 1.9243189627993598,
"height": 1.629934978730058,
"track_label_uuid": "bb0f40e4f68043e285d64a839f2f092c",
"timestamp": 315970611820366000,
"label_class": "VEHICLE",
}
log_city_name = "PIT"
lidar_ts = 315970611820366000
dataset_dir = TEST_DATA_LOC / "roi_based_test"
log_id = "21e37598-52d4-345c-8ef9-03ae19615d3d"
city_SE3_egovehicle = get_city_SE3_egovehicle_at_sensor_t(lidar_ts, dataset_dir, log_id)
dts = np.array([json_label_dict_to_obj_record(item) for item in [outside_obj, inside_obj]])
dts_filtered = filter_objs_to_roi(dts, avm, city_SE3_egovehicle, log_city_name)
assert dts_filtered.size == 1
assert dts_filtered.dtype == "O" # array of objects
assert isinstance(dts_filtered, np.ndarray)
assert dts_filtered[0].track_id == "bb0f40e4f68043e285d64a839f2f092c"
def project_lidar_to_img_motion_compensated(
pts_h_lidar_time: np.ndarray,
calib_data: Dict[str, Any],
camera_name: str,
cam_timestamp: int,
lidar_timestamp: int,
dataset_dir: str,
log_id: str,
return_K: bool = False,
) -> Union[_ReturnWithOptConfig, _ReturnWithoutOptConfig]:
"""
Because of the high frame rate, motion compensation's role between the
sensors is not very significant, moving points only by millimeters
to centimeters. If the vehicle is moving at 25 miles per hour, equivalent
to 11 meters/sec, then in 17 milliseconds (the max time between a lidar sweep
and camera image capture) we should be able to move up to 187 millimeters.
This can be verified in practice as the mean_change:
mean_change = np.amax(pts_h_cam_time.T[:,:3] - pts_h_lidar_time ,axis=0)
Adjust LiDAR points for ego-vehicle motion. This function accepts the
egovehicle's pose in the city map both at camera time and also at
the LiDAR time.
We perform the following transformation, where "ego" stands for
egovehicle reference frame
pt_ego_cam_t = ego_cam_t_SE3_map * map_SE3_ego_lidar_t * pt_ego_lidar_t
Note that both "cam_time_pts_h" and "lidar_time_pts_h" are 3D points in the
vehicle coordinate frame, but captured at different times. These LiDAR points
always live in the vehicle frame, but just in different timestamps. If we take
a lidar point in the egovehicle frame, captured at lidar time, and bring it into
the map at this lidar timestamp, then we know the transformation from map to
egovehicle reference frame at the time when the camera image was captured.
Thus, we move from egovehicle @ lidar time, to the map (which is time agnostic),
then we move from map to egovehicle @camera time. Now we suddenly have lidar points
living in the egovehicle frame @ camera time.
Args:
pts_h_lidar_time: Numpy array of shape (4,N)
calib_data: Python dictionary
camera_name: string, representing name of camera
cam_timestamp: integer, representing time in nanoseconds when
camera image was recorded
lidar_timestamp: integer, representing time in nanoseconds when
LiDAR sweep was recorded
dataset_dir: string, representing path to where dataset is stored
log_id: string, representing unique ID of vehicle log
return_K: return a copy of the
Returns:
uv: Numpy array of shape (N,2) with dtype np.float32
uv_cam: Numpy array of shape (N,3) with dtype np.float32
valid_pts_bool: Numpy array of shape (N,) with dtype bool
"""
# get transformation to bring point in egovehicle frame to city frame,
# at the time when camera image was recorded.
city_SE3_ego_cam_t = get_city_SE3_egovehicle_at_sensor_t(
cam_timestamp, dataset_dir, log_id)
# get transformation to bring point in egovehicle frame to city frame,
# at the time when the LiDAR sweep was recorded.
city_SE3_ego_lidar_t = get_city_SE3_egovehicle_at_sensor_t(
lidar_timestamp, dataset_dir, log_id)
if city_SE3_ego_cam_t is None or city_SE3_ego_lidar_t is None:
if return_K:
return None, None, None, None
else:
return None, None, None
# convert back from homogeneous
pts_h_lidar_time = pts_h_lidar_time.T[:, :3]
ego_cam_t_SE3_ego_lidar_t = city_SE3_ego_cam_t.inverse(
).right_multiply_with_se3(city_SE3_ego_lidar_t)
pts_h_cam_time = ego_cam_t_SE3_ego_lidar_t.transform_point_cloud(
pts_h_lidar_time)
pts_h_cam_time = point_cloud_to_homogeneous(pts_h_cam_time).T
return project_lidar_to_img(pts_h_cam_time, calib_data, camera_name,
return_K)
def accumulate(
dt_root_fpath: Path, gt_fpath: Path, cfg: DetectionCfg,
avm: Optional[ArgoverseMap]
) -> Tuple[DefaultDict[str, np.ndarray], DefaultDict[str, int]]:
"""Accumulate the true/false positives (boolean flags) and true positive errors for each class.
Args:
dt_root_fpath: Detections root folder file path.
gt_fpath: Ground truth file path.
cfg: Detection configuration.
Returns:
cls_to_accum: Class to accumulated statistics dictionary of shape |C| -> (N, K + S) where C
is the number of detection classes, K is the number of true positive thresholds used for
AP computation, and S is the number of true positive errors.
cls_to_ninst: Mapping of shape |C| -> (1,) the class names to the number of instances in the ground
truth dataset.
"""
log_id = gt_fpath.parents[1].stem
logger.info(f"log_id = {log_id}")
ts = int(gt_fpath.stem.split("_")[-1])
dt_fpath = dt_root_fpath / f"{log_id}/per_sweep_annotations_amodal/" f"tracked_object_labels_{ts}.json"
dts = np.array(read_label(str(dt_fpath)))
gts = np.array(read_label(str(gt_fpath)))
if cfg.eval_only_roi_instances and avm is not None:
# go up 3 levels, because hierarchy is as follows:
# {gt_root_fpath}/{log_id}/per_sweep_annotations_amodal/{gt_root_fname}
gt_root_fpath = Path(gt_fpath).parents[2]
city_SE3_egovehicle = get_city_SE3_egovehicle_at_sensor_t(
ts, str(gt_root_fpath), log_id)
if city_SE3_egovehicle is not None:
log_city_name = read_city_name(
os.path.join(gt_root_fpath, log_id, "city_info.json"))
dts = filter_objs_to_roi(dts, avm, city_SE3_egovehicle,
log_city_name)
gts = filter_objs_to_roi(gts, avm, city_SE3_egovehicle,
log_city_name)
cls_to_accum = defaultdict(list)
cls_to_ninst = defaultdict(int)
for class_name in cfg.dt_classes:
dt_filtered = filter_instances(
dts,
class_name,
filter_metric=cfg.dt_metric,
max_detection_range=cfg.max_dt_range,
)
gt_filtered = filter_instances(
gts,
class_name,
filter_metric=cfg.dt_metric,
max_detection_range=cfg.max_dt_range,
)
gt_filtered = remove_duplicate_instances(gt_filtered, cfg)
logger.info(f"{dt_filtered.shape[0]} detections")
logger.info(f"{gt_filtered.shape[0]} ground truth")
if dt_filtered.shape[0] > 0:
ranked_detections, scores = rank(dt_filtered)
metrics = assign(ranked_detections, gt_filtered, cfg)
cls_to_accum[class_name] = np.hstack((metrics, scores))
cls_to_ninst[class_name] = gt_filtered.shape[0]
return cls_to_accum, cls_to_ninst
def perform_SE3(cloud_dict, dataset=DATASET_DIR, log_id=LOG_ID):
for k, v in cloud_dict.items():
SE3 = get_city_SE3_egovehicle_at_sensor_t(str(k), dataset, log_id)
cloud_dict[k] = SE3.transform_point_cloud(v)
return cloud_dict
