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 run_ab3dmot(
classname: str,
pose_dir: str,
dets_dump_dir: str,
tracks_dump_dir: str,
min_conf: float = 0.3,
match_algorithm: str = 'h', #hungarian
match_threshold: float = 4,
match_distance: float = 'iou',
p: np.ndarray = np.eye(10),
thr_estimate: float = 0.8,
thr_prune: float = 0.1,
ps: float = 0.9) -> None:
"""
#path to argoverse tracking dataset test set, we will add our predicted labels into per_sweep_annotations_amodal/
#inside this folder
Filtering occurs in the city frame, not the egovehicle frame.
Args:
- classname: string, either 'VEHICLE' or 'PEDESTRIAN'
- pose_dir: string
- dets_dump_dir: string
- tracks_dump_dir: string
- max_age: integer
- min_hits: integer
Returns:
- None
"""
dl = SimpleArgoverseTrackingDataLoader(data_dir=pose_dir,
labels_dir=dets_dump_dir)
am = ArgoverseMap()
for log_id in tqdm(dl.sdb.get_valid_logs()):
print(log_id)
city_name = dl.get_city_name(log_id)
labels_folder = dets_dump_dir + "/" + log_id + "/per_sweep_annotations_amodal/"
lis = os.listdir(labels_folder)
lidar_timestamps = [
int(file.split(".")[0].split("_")[-1]) for file in lis
]
lidar_timestamps.sort()
previous_frame_bbox = []
ab3dmot = AB3DMOT(thr_estimate=thr_estimate,
thr_prune=thr_prune,
ps=ps)
print(labels_folder)
tracked_labels_copy = []
for j, current_lidar_timestamp in enumerate(lidar_timestamps):
dets = dl.get_labels_at_lidar_timestamp(log_id,
current_lidar_timestamp)
dets_copy = dets
transforms = []
city_SE3_egovehicle = dl.get_city_to_egovehicle_se3(
log_id, current_lidar_timestamp)
egovehicle_SE3_city = city_SE3_egovehicle.inverse()
transformed_labels = []
conf = []
for l_idx, l in enumerate(dets):
if l['label_class'] != classname:
# will revisit in other tracking pass
continue
if l["score"] < min_conf:
# print('Skipping det with confidence ', l["score"])
continue
det_obj = json_label_dict_to_obj_record(l)
det_corners_egovehicle_fr = det_obj.as_3d_bbox()
transforms += [city_SE3_egovehicle]
if city_SE3_egovehicle is None:
print('Was None')
# convert detection from egovehicle frame to city frame
det_corners_city_fr = city_SE3_egovehicle.transform_point_cloud(
det_corners_egovehicle_fr)
ego_xyz = np.mean(det_corners_city_fr, axis=0)
# Check the driveable/roi area
#da = am.remove_non_driveable_area_points(np.array([ego_xyz]), city_name=city_name)
# if len(da) == 0 and l['label_class'] == 'VEHICLE':
# continue
# roi = am.remove_non_roi_points(np.array([ego_xyz]), city_name=city_name)
# if len(roi) == 0:
# continue
yaw = yaw_from_bbox_corners(det_corners_city_fr)
transformed_labels += [[
ego_xyz[0], ego_xyz[1], ego_xyz[2], yaw, l["length"],
l["width"], l["height"]
]]
conf += [l["score"]]
if len(transformed_labels) > 0:
transformed_labels = np.array(transformed_labels)
else:
transformed_labels = np.empty((0, 7))
dets_all = {
"dets": transformed_labels,
"info": np.zeros(transformed_labels.shape),
"conf": conf
}
# perform measurement update in the city frame.
dets_with_object_id = ab3dmot.update(dets_all, match_distance,
match_threshold,
match_algorithm, p)
tracked_labels = []
for det in dets_with_object_id:
# move city frame tracks back to ego-vehicle frame
xyz_city = np.array(
[det[0].item(), det[1].item(),
det[2].item()]).reshape(1, 3)
city_yaw_object = det[3]
city_se2_object = SE2(rotation=rotmat2d(city_yaw_object),
translation=xyz_city.squeeze()[:2])
city_se2_egovehicle, city_yaw_ego = get_B_SE2_A(
city_SE3_egovehicle)
ego_se2_city = city_se2_egovehicle.inverse()
egovehicle_se2_object = ego_se2_city.right_multiply_with_se2(
city_se2_object)
# recreate all 8 points
# transform them
# compute yaw from 8 points once more
egovehicle_SE3_city = city_SE3_egovehicle.inverse()
xyz_ego = egovehicle_SE3_city.transform_point_cloud(
xyz_city).squeeze()
# update for new yaw
# transform all 8 points at once, then compute yaw on the fly
ego_yaw_obj = se2_to_yaw(egovehicle_se2_object)
qx, qy, qz, qw = yaw_to_quaternion3d(ego_yaw_obj)
tracked_labels.append({
"center": {
"x": xyz_ego[0],
"y": xyz_ego[1],
"z": xyz_ego[2]
},
"rotation": {
"x": qx,
"y": qy,
"z": qz,
"w": qw
},
"length":
det[4],
"width":
det[5],
"height":
det[6],
"track_label_uuid":
uuid_gen.get_uuid(det[7]),
"timestamp":
current_lidar_timestamp,
"label_class":
classname
})
tracked_labels_copy = copy.deepcopy(tracked_labels)
label_dir = os.path.join(tracks_dump_dir, log_id,
"per_sweep_annotations_amodal")
check_mkdir(label_dir)
json_fname = f"tracked_object_labels_{current_lidar_timestamp}.json"
json_fpath = os.path.join(label_dir, json_fname)
if Path(json_fpath).exists():
# accumulate tracks of another class together
prev_tracked_labels = read_json_file(json_fpath)
tracked_labels.extend(prev_tracked_labels)
save_json_dict(json_fpath, tracked_labels)
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 run_tracking(classname, labels_folder, output):
lis = os.listdir(labels_folder)
lidar_timestamps = [int(f.split(".")[0]) for f in lis]
lidar_timestamps.sort()
ab3dmot = AB3DMOT()
# print(labels_folder)
for i, stamp in tqdm(enumerate(lidar_timestamps)):
with open(labels_folder + "/" + str(stamp) + ".json") as dets_file:
j_file = json.load(dets_file)
# print(j_file)
# Process each detection instances
labels = []
for det in j_file:
det["center"]["x"] = float(det["center"]["x"])
det["center"]["y"] = float(det["center"]["y"])
det["center"]["z"] = float(det["center"]["z"])
det["length"] = float(det["length"])
det["width"] = float(det["width"])
det["height"] = float(det["height"])
det_obj = json_label_dict_to_obj_record(det)
# print(det)
det_bbox = det_obj.as_3d_bbox()
yaw = yaw_from_bbox_corners(det_bbox)
labels.append([
det_obj.translation[0], det_obj.translation[0],
det_obj.translation[0], yaw, det["length"], det["width"],
det["height"]
])
labels = np.array(labels)
dets_all = {"dets": labels, "info": np.zeros(labels.shape)}
# print(dets_all)
dets_with_object_id = ab3dmot.update(dets_all, classname)
# print(dets_with_object_id)
tracked_labels = []
for det in dets_with_object_id:
qx, qy, qz, qw = yaw_to_quaternion3d(det[3])
tracked_labels.append({
"center": {
"x": det[0],
"y": det[1],
"z": det[2]
},
"rotation": {
"x": qx,
"y": qy,
"z": qz,
"w": qw
},
"length": det[4],
"width": det[5],
"height": det[6],
"track_label_uuid": det[7],
"timestamp": stamp,
"label_class": classname
})
# print(tracked_labels)
if not os.path.exists(output):
os.mkdir(output)
output_name = os.path.join(output, str(stamp) + ".json")
if os.path.exists(output_name):
prev_labels = read_json_file(output_name)
tracked_labels.extend(prev_labels)
save_json_dict(output_name, tracked_labels)
