"--kitti-path",
dest="kitti_path",
default="./data/KITTI/tracking/training/",
)
parser.add_argument(
"-s",
"--scene",
dest="scene",
help="the scene to plot",
choices=list(map(str, range(0, 20))),
)
args = parser.parse_args()
scene = args.scene.zfill(4)
kitti_path = Path(args.kitti_path)
gt_poses_world = get_gt_poses_from_kitti(kitti_path, scene)
get_cameras_from_kitti(kitti_path)
label_data = get_gt_detection_data_from_kitti(
kitti_path,
scene,
gt_poses_world,
offset=0,
)
fig = plt.figure()
ax_3d = fig.add_subplot(1, 1, 1, projection="3d")
gt_poses_x, gt_poses_y, gt_poses_z = zip(
*(map(lambda x: x[:3, 3], gt_poses_world)))
ax_3d.plot(gt_poses_x, gt_poses_y, gt_poses_z, linestyle="solid")
for track_data in label_data.values():
x, y, z = zip(*list(row.world_pos for row in track_data.values()))
x = [e[0] for e in x]
y = [e[0] for e in y]
queue_class = queue.Queue
flag_class = threading.Event
process_class = threading.Thread
shared_data = queue_class()
returned_data = queue_class()
writer_data_2d = queue_class()
writer_data_3d = queue_class()
slam_data = queue_class()
stop_flag = flag_class()
next_step = flag_class()
img_shape = get_image_shape(kitti_path, scene)
image_stream = _get_image_stream(kitti_path,
scene,
stop_flag,
offset=args.offset)
stereo_cam, T02 = get_cameras_from_kitti(kitti_path, scene)
gt_poses = get_gt_poses_from_kitti(kitti_path, scene)
detection_stream = get_detection_stream(
obj_detections_path,
scene=scene,
offset=args.offset,
object_ids=[int(idx)
for idx in args.indeces] if args.indeces else None,
classes=args.classes,
)
slam_process = process_class(target=_fake_slam,
args=[slam_data, gt_poses, args.offset],
name="Fake SLAM")
write_2d_process = process_class(
target=_write_2d_detections,
def _process_scene(scene):
scene = str(scene).zfill(4)
kitti_path = Path(config.KITTI_PATH)
if not args.no_save:
if args.o:
save_path = Path(args.o)
else:
save_path = kitti_path
if args.use_gt:
save_path_detections = save_path / "stereo_detections_gt"
else:
save_path_detections = save_path / "stereo_detections"
if args.save_slam:
if args.use_gt:
save_path_slam = save_path / "masked_slam_input_gt"
else:
save_path_slam = save_path / "masked_slam_input"
save_path_slam_left = save_path_slam / "image_02" / scene
save_path_slam_right = save_path_slam / "image_03" / scene
save_path_slam_left.mkdir(parents=True, exist_ok=True)
save_path_slam_right.mkdir(parents=True, exist_ok=True)
save_path_mot_left = save_path_detections / "image_02"
save_path_mot_right = save_path_detections / "image_03"
left_mot_out_file = save_path_mot_left / (scene + ".txt")
if left_mot_out_file.exists():
left_mot_out_file.unlink() # overwrite file
right_mot_out_file = save_path_mot_right / (scene + ".txt")
if right_mot_out_file.exists():
right_mot_out_file.unlink() # overwrite file
save_path_mot_left.mkdir(parents=True, exist_ok=True)
save_path_mot_right.mkdir(parents=True, exist_ok=True)
stereo_cam, T02 = get_cameras_from_kitti(kitti_path, scene)
stereo_cam.T_left_right[0, 3] = 0.03
image_stream = get_image_stream(kitti_path, scene, with_file_names=True)
if not args.no_view:
width, height = get_screen_size()
cv2.namedWindow("Preprocessed", cv2.WINDOW_NORMAL)
cv2.resizeWindow("Preprocessed", (width // 2, height // 2))
if not args.no_right:
all_right_object_detections = get_estimated_obj_detections(
kitti_path, scene, "right")
else:
all_right_object_detections = {}
if not args.use_gt:
all_left_object_detections = get_estimated_obj_detections(
kitti_path, scene, "left")
colors = defaultdict(lambda: get_color(normalized=False, as_tuple=True))
for idx, (stereo_image, filenames) in tqdm.tqdm(
enumerate(image_stream),
total=len(image_stream),
position=int(scene),
leave=False,
):
right_object_detections = all_right_object_detections.get(idx, [])
left_fname, right_fname = filenames
if args.use_gt:
left_object_detections = get_gt_obj_detections_from_kitti(
kitti_path, scene, idx)
else:
left_object_detections = all_left_object_detections.get(idx, [])
masked_stereo_image_slam, stereo_object_detections = preprocess_frame(
stereo_image,
left_object_detections=left_object_detections,
right_object_detections=right_object_detections,
only_iou=args.only_iou,
use_unmatched=args.use_unmatched,
colors=colors,
)
left_mot_mask = masked_stereo_image_slam.left == 0
right_mot_mask = masked_stereo_image_slam.right == 0
left_img_mot = 255 * np.ones(stereo_image.left.shape, dtype=np.uint8)
left_img_mot[left_mot_mask] = stereo_image.left[left_mot_mask]
right_img_mot = 255 * np.ones(stereo_image.right.shape, dtype=np.uint8)
right_img_mot[right_mot_mask] = stereo_image.right[right_mot_mask]
masked_stereo_image_mot = StereoImage(
left_img_mot,
right_img_mot,
img_width=stereo_image.img_width,
img_height=stereo_image.img_height,
)
result_slam = np.hstack(
[masked_stereo_image_slam.left, masked_stereo_image_slam.right])
result_slam = cv2.putText(
result_slam,
"/".join(left_fname.split("/")[-3:]),
(5, 25),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1,
color=(0, 255, 0),
thickness=2,
)
result_slam = cv2.putText(
result_slam,
"/".join(right_fname.split("/")[-3:]),
(5 + right_img_mot.shape[1], 25),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1,
color=(0, 255, 0),
thickness=2,
)
result_mot = np.hstack(
[masked_stereo_image_mot.left, masked_stereo_image_mot.right])
result = np.vstack([result_slam, result_mot])
if not args.no_view:
cv2.imshow("Preprocessed", result)
pressed_key = cv2.waitKey(1)
if args.no_continue:
while pressed_key not in [ord("q"), ord("n")]:
pressed_key = cv2.waitKey(1)
if pressed_key == ord("q"):
cv2.destroyAllWindows()
break
if not args.no_save:
img_name = left_fname.split("/")[-1]
img_id = img_name.split(".")[0]
if args.save_slam:
slam_left_path = save_path_slam_left / img_name
slam_right_path = save_path_slam_right / img_name
cv2.imwrite(slam_left_path.as_posix(),
masked_stereo_image_slam.left)
cv2.imwrite(slam_right_path.as_posix(),
masked_stereo_image_slam.right)
for detection in stereo_object_detections:
track_id = detection.left.track_id
obj_cls = detection.left.cls
img_height = stereo_image.img_height
img_width = stereo_image.img_width
left_track_line = get_2d_track_line(
img_id=img_id,
track_id=track_id,
obj_cls=obj_cls,
height=img_height,
width=img_width,
mask=detection.left.mask,
)
right_track_line = get_2d_track_line(
img_id=img_id,
track_id=track_id,
obj_cls=obj_cls,
height=img_height,
width=img_width,
mask=detection.right.mask,
)
with open(left_mot_out_file, "a") as fp:
fp.write(left_track_line + "\n")
with open(right_mot_out_file, "a") as fp:
fp.write(right_track_line + "\n")