def landmarks(object_points, object_poses, camera_params):
# go through all object points
lms = {}
for i, pt_obj in enumerate(object_points):
# for every object pose, decide whether point was seen
# transform point to current pose
# project onto camera + add some noise
observations = []
for pose_id, pose in object_poses.items():
if RNG.normal(0.8, 0.5) <= 0.5:
# point isn't observed at this pose
continue
pt_3d = from_homogeneous(pose @ to_homogeneous(pt_obj))
pt_2d = (project(camera_params, pt_3d) + RNG.random() * 2).reshape(
(2, )) # up to two pixel error
observations.append(
Observation(descriptor=None,
pt_2d=pt_2d,
timecam_id=(pose_id, 0)))
landmark = Landmark(pt_obj, observations)
lms[i] = landmark
return lms
def _write_3d_detections(
writer_data_3d: Union[queue.Queue, mp.Queue],
scene: str,
kitti_path: Path,
tags: List[str],
):
path = kitti_path / "3d_tracking"
for tag in tags:
path /= tag
path /= "data" # to adhere to dir structure expected by ab3dmot
path.mkdir(parents=True, exist_ok=True)
fname = path / (scene + ".txt")
# overwrite file if exists
if fname.exists():
fname.unlink()
while True:
with open(fname, "a") as fp:
track_data = writer_data_3d.get(block=True)
writer_data_3d.task_done()
if not track_data:
LOGGER.debug("Finished writing 3d detections")
break
img_id = track_data["img_id"]
LOGGER.debug("Got 3d detection data for image %d", img_id)
T_world_cam = track_data["T_world_cam"]
for track_id, track in track_data["tracks"].items():
obj_type = track.cls.lower()
dims = config.PED_DIMS if obj_type == "pedestrian" else config.CAR_DIMS
location = track.locations.get(img_id)
if location is None:
continue
rot_angle = np.array(track.rot_angle.get(img_id))
mask = track.masks[0]
y_top_left, x_top_left = map(min, np.where(mask != 0))
y_bottom_right, x_bottom_right = map(max, np.where(mask != 0))
bbox2d = [
x_top_left, y_top_left, x_bottom_right, y_bottom_right
]
# beta is angle between z and location dir vector
loc_cam = from_homogeneous(
np.linalg.inv(T_world_cam) @ to_homogeneous(location))
# kitti locations are given as bottom of bounding box
# positive y direction is downward, hence add half of the dimensions
loc_cam[1] += dims[0] / 2
dir_vec = loc_cam[[0, 2]].reshape(2, 1)
dir_vec /= np.linalg.norm(dir_vec)
beta = np.arccos(
np.dot(dir_vec.T,
np.array([0, 1]).reshape(2, 1)))
if dir_vec[0] < 0:
beta = -beta
if not np.isfinite(rot_angle):
rot_angle = np.array([0])
alpha = rot_angle - beta
# number of badly tracked frames negatively effects confidence
btf_score = get_confidence(
max_point=config.KEEP_TRACK_FOR_N_FRAMES_AFTER_LOST,
value=track.badly_tracked_frames,
upward_sloping=False,
)
# number of landmarks positively effects confidence
num_lm_score = get_confidence(max_point=120,
value=len(track.landmarks))
# number of poses positively effects confidence
num_pose_score = get_confidence(max_point=10,
value=len(track.poses))
# dist from camera negatively effects confidence
dist_cam_score = get_confidence(
max_point=100,
value=track.dist_from_cam,
upward_sloping=False,
)
confidence_score = (btf_score * num_lm_score * num_pose_score *
dist_cam_score)
line = get_3d_track_line(
img_id=img_id,
track_id=track_id,
obj_type=obj_type,
dims=dims,
loc=loc_cam.flatten().tolist(),
rot=rot_angle.flatten()[0],
bbox_2d=bbox2d,
confidence_score=confidence_score,
alpha=alpha.flatten()[0],
)
fp.write(line + "\n")
def get_gt_detection_data_from_kitti(
kitti_path: Union[str, Path],
scene: Union[str, int],
poses: List[np.ndarray],
offset: int = 0,
indexed_by_image_id: bool = False,
) -> DetectionData:
kitti_path = _convert_to_path_if_needed(kitti_path)
scene = _convert_to_str_scene(scene)
detection_file = _get_detection_file(kitti_path, scene)
if not detection_file.exists():
raise FileNotFoundError(
f"Detection file {detection_file.as_posix()} doesn't exist"
)
detection_data: DetectionData = {}
with open(detection_file, "r") as fp:
for line in fp:
cols = line.split(" ")
frame = int(cols[0])
if frame < offset:
continue
track_id = int(cols[1])
if track_id == -1:
continue
object_class = str(cols[2])
if object_class.lower() not in ["car", "pedestrian"]:
continue
truncation_level = int(cols[3])
occlusion_level = int(cols[4])
# cols[5] is observation angle of object
bbox = list(map(float, cols[6:10])) # in left image coordinates
dim_3d = list(map(float, cols[10:13])) # in camera coordinates
location_cam2 = np.array(list(map(float, cols[13:16]))).reshape(
(3, 1)
) # in camera coordinates
rot_angle = float(cols[16]) # in camera coordinates
T_w_cam2 = poses[frame]
dir_vec = location_cam2[[0, 2]].reshape(2, 1)
dir_vec /= np.linalg.norm(dir_vec)
beta = np.arccos(np.dot(dir_vec.T, np.array([0, 1]).reshape(2, 1)))
if dir_vec[0] < 0:
beta = -beta
location_world = from_homogeneous(T_w_cam2 @ to_homogeneous(location_cam2))
if indexed_by_image_id:
if not detection_data.get(frame):
detection_data[frame] = {}
else:
if not detection_data.get(track_id):
detection_data[track_id] = {}
detection_row = DetectionDataRow(
world_pos=location_world.reshape(-1).tolist(),
cam_pos=location_cam2.reshape(-1).tolist(),
occ_lvl=occlusion_level,
trunc_lvl=truncation_level,
object_class=object_class,
bbox2d=bbox,
dim_3d=dim_3d,
rot_angle=rot_angle,
)
if indexed_by_image_id:
detection_data[frame][track_id] = detection_row
else:
detection_data[track_id][frame] = detection_row
LOGGER.debug("Extracted trajectories for %d objects", len(detection_data))
return detection_data
def _write_obb_data(
writer_obb_data: Union[queue.Queue, mp.Queue],
scene: str,
kitti_path: Path,
tags: List[str],
):
path = kitti_path / "obb_data"
for tag in tags:
path /= tag
path.mkdir(parents=True, exist_ok=True)
pcl_dir = path / scene
pcl_dir.mkdir(exist_ok=True)
fname = path / (scene + ".csv")
columns = [
"scene",
"img_id",
"track_id",
"x",
"y",
"z",
"yaw",
"num_poses",
"badly_tracked_frames",
"num_points",
"dist_from_cam",
]
# overwrite file if exists
with open(fname, "w") as fp:
fp.write(",".join(columns) + "\n")
while True:
with open(fname, "a") as fp:
track_data = writer_obb_data.get(block=True)
writer_obb_data.task_done()
img_id = track_data["img_id"]
LOGGER.debug("Got 3d obb data for image %d", img_id)
T_world_cam = track_data["T_world_cam"]
dims = config.CAR_DIMS
for track_id, track in track_data["tracks"].items():
dist_from_cam = track.dist_from_cam
pos = track.locations.get(img_id)
if pos is None:
continue
rot_angle = np.array(track.rot_angle.get(img_id))
if not np.isfinite(rot_angle):
rot_angle = np.array([0])
pos_cam = from_homogeneous(
np.linalg.inv(T_world_cam) @ to_homogeneous(pos))
# kitti locations are given as bottom of bounding box
# positive y direction is downward, hence add half of the dimensions
pos_cam[1] += dims[0] / 2
num_poses = len(track.poses)
num_landmarks = len(track.landmarks)
badly_tracked_frames = track.badly_tracked_frames
line = (
f"{scene}, {img_id}, {track_id}, {', '.join(map(str, pos_cam.flatten().tolist()))}, {rot_angle.flatten().tolist()[0]}, {num_poses}, "
f"{badly_tracked_frames}, {num_landmarks}, {dist_from_cam}"
)
fp.write(line + "\n")
pcl_fname = pcl_dir / f"{img_id}_{track_id}.npy"
points = np.array([
lm.pt_3d for lm in track.landmarks.values()
]).reshape(-1, 3)
np.save(pcl_fname, points)
def _compute_bounding_box(location, rot_angle, dimensions, T_world_cam):
vec = np.array([*location, rot_angle, *dimensions]).reshape(7, 1)
corners_cam = get_corners_from_vector(vec)
corners_world = from_homogeneous(T_world_cam @ to_homogeneous(corners_cam))
return _get_points_and_lines_from_corners(corners_world)
def _update_track_visualization(
all_track_geometries,
all_geometries,
object_tracks,
visualizer,
show_online_trajs,
show_offline_trajs,
current_img_id,
cached_colors,
current_cam_pose,
):
# display current track estimates
inactive_tracks = []
for ido in all_track_geometries:
if ido not in object_tracks:
inactive_tracks.append(ido)
for ido, track in object_tracks.items():
track_geometries = all_track_geometries.get(
ido,
TrackGeometries(
pt_cloud=o3d.geometry.PointCloud(),
offline_trajectory=o3d.geometry.LineSet(),
online_trajectory=o3d.geometry.LineSet(),
bbox=o3d.geometry.LineSet(),
color=cached_colors.get(ido, get_color(only_bright=True)),
),
)
cached_colors[ido] = track_geometries.color
# draw path
path_points_offline = []
path_points_online = []
points = []
color = track_geometries.color
lighter_color = color + 0.25 * np.array(Colors.WHITE)
darker_color = color - 0.25 * np.array(Colors.WHITE)
lighter_color = np.clip(lighter_color, 0, 1)
darker_color = np.clip(darker_color, 0, 1)
track_size = 0
dimensions = config.CAR_DIMS if track.cls == "car" else config.PED_DIMS
for i, (img_id, pose_world_obj) in enumerate(track.poses.items()):
# center = np.array([0.0, 0.0, 0.0]).reshape(3, 1)
if i == len(track.poses) - 1:
for lm in track.landmarks.values():
pt_world = from_homogeneous(
pose_world_obj @ to_homogeneous(lm.pt_3d))
# Ecenter += pt_world
if i == len(
track.poses) - 1 and np.isfinite(pt_world).all():
points.append(pt_world)
if len(points) < 3:
continue
try:
if track.rot_angle.get(current_img_id) is not None:
location = from_homogeneous(
np.linalg.inv(current_cam_pose) @ to_homogeneous(
track.locations[current_img_id]))
# kitti locations are given as bottom of bounding box
# positive y direction is downward, hence add half of the dimensions
location[1] += dimensions[0] / 2
bbox, lines = _compute_bounding_box(
location,
track.rot_angle[current_img_id],
dimensions,
current_cam_pose,
)
track_geometries.bbox.points = bbox
track_geometries.bbox.lines = lines
else:
tmp_pt_cloud = o3d.geometry.PointCloud()
tmp_pt_cloud.points = o3d.utility.Vector3dVector(
points)
bbox = tmp_pt_cloud.get_oriented_bounding_box()
track_geometries.bbox.points = bbox.get_box_points()
track_geometries.bbox.lines = o3d.utility.Vector2iVector(
[
[0, 1],
[0, 2],
[0, 3],
[1, 6],
[1, 7],
[2, 5],
[2, 7],
[3, 5],
[3, 6],
[4, 5],
[4, 6],
[4, 7],
])
except RuntimeError:
# happens when points are too close to each other
pass
# if track.landmarks:
# center /= len(track.landmarks)
# offline_point = center.reshape(3,).tolist()
offline_point = track.pcl_centers.get(img_id)
if offline_point is not None:
pt_world = from_homogeneous(
pose_world_obj @ to_homogeneous(offline_point))
pt_world[2] -= dimensions[0] / 2
path_points_offline.append(pt_world)
online_point = track.locations.get(img_id).copy()
if online_point is not None:
online_point[2] -= dimensions[0] / 2
path_points_online.append(online_point.reshape(3, ).tolist())
if i == len(track.poses) - 1:
track_size = len(points)
path_lines_offline = [[i, i + 1]
for i in range(len(path_points_offline) - 1)]
path_lines_online = [[i, i + 1]
for i in range(len(path_points_online) - 1)]
# draw current landmarks
LOGGER.debug("Track has %d points", track_size)
track_geometries.pt_cloud.points = o3d.utility.Vector3dVector(points)
if path_lines_offline:
track_geometries.offline_trajectory.points = o3d.utility.Vector3dVector(
path_points_offline)
track_geometries.offline_trajectory.lines = o3d.utility.Vector2iVector(
path_lines_offline)
if path_lines_online:
track_geometries.online_trajectory.points = o3d.utility.Vector3dVector(
path_points_online)
track_geometries.online_trajectory.lines = o3d.utility.Vector2iVector(
path_lines_online)
if track.active:
track_geometries.pt_cloud.paint_uniform_color(color)
if show_offline_trajs.val:
track_geometries.offline_trajectory.paint_uniform_color(color)
else:
track_geometries.offline_trajectory.paint_uniform_color(
VIEWER_COLORS.background)
if show_online_trajs.val:
track_geometries.online_trajectory.paint_uniform_color(
darker_color)
else:
track_geometries.online_trajectory.paint_uniform_color(
VIEWER_COLORS.background)
track_geometries.bbox.paint_uniform_color(color)
else:
LOGGER.debug("Track is inactive")
track_geometries.pt_cloud.paint_uniform_color(
VIEWER_COLORS.background)
track_geometries.offline_trajectory.paint_uniform_color(
VIEWER_COLORS.background)
track_geometries.online_trajectory.paint_uniform_color(
VIEWER_COLORS.background)
track_geometries.bbox.paint_uniform_color(VIEWER_COLORS.background)
if all_track_geometries.get(ido) is None:
visualizer.add_geometry(track_geometries.pt_cloud,
reset_bounding_box=False)
visualizer.add_geometry(track_geometries.bbox,
reset_bounding_box=False)
all_geometries.add(track_geometries.pt_cloud)
all_geometries.add(track_geometries.bbox)
if (len(path_lines_online) >= 1 and
track_geometries.online_trajectory not in all_geometries):
visualizer.add_geometry(track_geometries.online_trajectory,
reset_bounding_box=False)
all_geometries.add(track_geometries.online_trajectory)
if (len(path_lines_offline) >= 1 and
track_geometries.offline_trajectory not in all_geometries):
visualizer.add_geometry(track_geometries.offline_trajectory,
reset_bounding_box=False)
all_geometries.add(track_geometries.offline_trajectory)
all_track_geometries[ido] = track_geometries
else:
visualizer.update_geometry(track_geometries.pt_cloud)
if (len(path_lines_online) >= 1 and
track_geometries.online_trajectory not in all_geometries):
visualizer.add_geometry(track_geometries.online_trajectory,
reset_bounding_box=False)
all_geometries.add(track_geometries.online_trajectory)
else:
visualizer.update_geometry(track_geometries.online_trajectory)
if (len(path_lines_offline) >= 1 and
track_geometries.offline_trajectory not in all_geometries):
visualizer.add_geometry(track_geometries.offline_trajectory,
reset_bounding_box=False)
all_geometries.add(track_geometries.offline_trajectory)
else:
visualizer.update_geometry(track_geometries.offline_trajectory)
visualizer.update_geometry(track_geometries.bbox)
for ido in inactive_tracks:
track_geometry = all_track_geometries.get(ido)
if track_geometry is not None:
cached_colors[ido] = track_geometry.color
visualizer.remove_geometry(track_geometry.pt_cloud,
reset_bounding_box=False)
visualizer.remove_geometry(track_geometry.bbox,
reset_bounding_box=False)
visualizer.remove_geometry(track_geometry.offline_trajectory,
reset_bounding_box=False)
visualizer.remove_geometry(track_geometry.online_trajectory,
reset_bounding_box=False)
all_geometries.remove(track_geometry.pt_cloud)
all_geometries.remove(track_geometry.bbox)
if track_geometry.offline_trajectory in all_geometries:
all_geometries.remove(track_geometry.offline_trajectory)
if track_geometry.online_trajectory in all_geometries:
all_geometries.remove(track_geometry.online_trajectory)
del all_track_geometries[ido]
del cached_colors[ido]