def import_pose_graph(poses, names):
known = {}
relative = {}
def check_camera(k):
assert k in names, f"{k} not found in ({str(names)}"
def insert_relation(k, t):
check_camera(k)
r = relative.get(k, [])
relative[k] = r + [t]
for k, v in poses.items():
t = import_rt(v)
if "_to_" in k:
source, dest = k.split("_to_")
insert_relation(source,
struct(dest=dest, transform=np.linalg.inv(t)))
insert_relation(dest, struct(dest=source, transform=t))
else:
check_camera(k)
known[k] = t
poses = propagate_poses(list(known.items()), relative)
missing = set(names) - poses.keys()
assert len(
missing) == 0, f"import_rig: missing camera poses for {str(missing)}"
return poses
def set_calibration(self, index):
with (self.updating):
ws = self.workspace
assert index < len(ws.calibrations)
_, calibs = split_dict(ws.calibrations)
self.calibration = calibs[index]
self.params_viewer.set_cameras(
self.calibration.cameras,
tables.inverse(self.calibration.camera_poses.pose_table))
self.viewer_3d.enable(False)
if self.controllers is None:
self.controllers = struct(
moving_cameras=MovingCameras(self.viewer_3d,
self.calibration,
ws.board_colors),
moving_board=MovingBoard(self.viewer_3d, self.calibration,
ws.board_colors))
else:
for controller in self.controllers.values():
controller.update_calibration(self.calibration)
self.setup_view_table(self.calibration)
self.viewer_3d.enable(True)
self.viewer_3d.fix_camera()
self.update_controller()
def pose_errors(p1, p2):
d = p1 @ np.linalg.inv(p2)
r, t = split(d)
return struct(translation=np.linalg.norm(t, axis=(1)),
rotation_deg=R.magnitude(R.from_matrix(r)) * 180.0 / math.pi,
frobius=np.linalg.norm(p1 - p2, axis=(1, 2)))
def export_single(filename, cameras, camera_names, filenames):
filenames = transpose_lists(filenames)
data = struct(cameras=export_cameras(camera_names, cameras),
image_sets=export_images(camera_names, filenames))
with open(filename, 'w') as outfile:
json.dump(to_dicts(data), outfile, indent=2)
def transform(time_poses):
pose_table = tables.expand_views(
struct(camera=camera_poses, times=time_poses))
return tables.transform_points(
tables.expand_dims(pose_table, (2, 3)),
tables.expand_dims(world_points, (0, 1)))
def write_detections(self, filename):
data = struct(filenames=self.filenames,
boards=self.boards,
image_sizes=self.image_sizes,
detected_points=self.detected_points)
with open(filename, "wb") as file:
pickle.dump(data, file)
def add_reprojections(scene, points, projected, inliers, boards, valid_boards,
options):
marker_font = QFont()
marker_font.setPixelSize(options.marker_size * 0.75)
frame_table = points._extend(proj=projected.points, inlier=inliers)
colors = struct(error_line=(1, 1, 0),
outlier=(1, 0, 0),
inlier=(0, 1, 0),
invalid=(0.5, 0.5, 0))
pens = colors._map(cosmetic_pen, options.line_width)
for board, valid_board, board_points in zip(boards, valid_boards,
frame_table._sequence(0)):
if not valid_board:
continue
for point, id in zip(board_points._sequence(), board.ids):
color_key = 'invalid' if not point.valid else (
'inlier' if point.inlier else 'outlier')
add_marker(scene, point.proj, id, options, pens[color_key],
colors[color_key], marker_font)
if point.valid:
scene.addItem(line(point.proj, point.points, pens.error_line))
def export(self):
return struct(type='charuco',
aruco_dict=self.aruco_dict,
aruco_offset=self.aruco_offset,
size=self.size,
marker_length=self.marker_length,
square_length=self.square_length,
aruco_params=self.aruco_params)
def error_stats(errors):
mse = np.square(errors).mean()
quantiles = np.array(
[np.quantile(errors, n) for n in [0, 0.25, 0.5, 0.75, 1]])
return struct(mse=mse,
rms=np.sqrt(mse),
quantiles=quantiles,
n=errors.size)
def project_points(pose_table, cameras, camera_poses, world_points):
pose_estimates = struct(camera=camera_poses, times=pose_table)
pose_table = tables.expand_views(pose_estimates)
points = tables.transform_points(tables.expand_dims(pose_table, (2, 3)),
tables.expand_dims(world_points, (0, 1)))
return project_cameras(cameras, points)
def export(self):
return struct(type='aprilgrid',
start_id=self.start_id,
tag_family=self.tag_family,
size=self.size,
tag_length=self.tag_length,
tag_spacing=self.tag_spacing,
border_bits=self.border_bits)
def intersect_detections(board, d1, d2):
ids, inds1, inds2 = np.intersect1d(d1.ids, d2.ids, return_indices=True)
if len(ids) > 0:
return struct(points1 = d1.corners[inds1], points2 = d2.corners[inds2],
object_points = board.points[ids], ids=ids)
else:
return None
def param_objects(self):
return struct(
camera_poses = self.camera_poses,
board_poses = self.board_poses,
motion = self.motion,
cameras = self.cameras,
boards = self.boards
)
def grid_mesh(points, size):
w, h = size
indices = np.arange(points.shape[0]).reshape(h - 1, w - 1)
quad = np.array(
[indices[0, 0], indices[1, 0], indices[1, 1], indices[0, 1]])
offsets = indices[:h - 2, :w - 2]
quads = quad.reshape(1, 4) + offsets.reshape(-1, 1)
return struct(points=points, polygons=quad_polygons(quads))
def params(self):
f = self.focal_length
if self.fix_aspect:
f = np.array([f.mean(), f.mean()])
return struct(focal_length=f,
principle_point=self.principle_point,
skew=np.array([self.skew]),
dist=self.dist)
def export(filename, calib, names, master=None):
if master is not None:
calib = calib.with_master(master)
camera_poses = calib.camera_poses.pose_table
data = struct(
cameras = export_cameras(names.camera, calib.cameras),
camera_poses = export_camera_poses(names.camera, camera_poses)\
if master is None else export_relative(names.camera, camera_poses, master),
image_sets = struct(
rgb = [{camera : path.join(camera, image) for camera in names.camera}
for image in names.image]
),
)
with open(filename, 'w') as outfile:
json.dump(to_dicts(data), outfile, indent=2)
def import_cameras(calib_data):
assert 'cameras' in calib_data, "import_cameras: no camera information in data"
cameras = {
k: import_camera(camera)
for k, camera in calib_data.cameras.items()
}
transforms = import_pose_graph(calib_data.camera_poses, cameras)\
if 'camera_poses' in calib_data else None
return struct(cameras=cameras, camera_poses=transforms)
def __init__(self, viewer, axis_mesh, poses):
options = struct(rgb=True, scalars='colors', show_edges=True)
self.instances = [
Marker(viewer, pv.PolyData(axis_mesh), pose, options=options)
for pose in poses._sequence()
]
for instance in self.instances:
instance.set_point_size(0)
def get_paths(args):
np.set_printoptions(precision=4, suppress=True)
output_path = args.output_path or args.image_path
pathlib.Path(output_path).mkdir(parents=True, exist_ok=True)
return struct(log_file=path.join(output_path, f"{args.name}.log"),
export_file=path.join(output_path, f"{args.name}.json"),
detection_cache=path.join(output_path, f"detections.pkl"),
workspace_file=path.join(output_path, f"{args.name}.pkl"))
def detect(self, image):
detections = self.grid.compute_observation(image)
if not detections.success:
return empty_detection
corner_detections = [struct(ids = id * 4 + k % 4, corners=corner)
for k, id, corner in zip(range(len(detections.ids)), detections.ids, detections.image_points)]
return subpix_corners(image, Table.stack(corner_detections), self.subpix_region)
def board_correspondences(board, detections):
non_empty = [d for d in detections if board.has_min_detections(d)]
if len(non_empty) == 0:
return struct(corners = [], object_points=[], ids=[])
detections = transpose_structs(non_empty)
return detections._extend(
object_points=[board.points[ids].astype(np.float32) for ids in detections.ids],
corners=[corners.astype(np.float32) for corners in detections.corners]
)
def state(self):
frame, camera = self.selection()
names = self.workspace.names
images = self.workspace.images
return struct(frame=frame,
camera=camera,
scale=self.camera_size(),
camera_name=names.camera[camera],
image_name=names.image[frame],
image=images[camera][frame])
def emit(self, record):
try:
msg = self.format(record)
entry = struct(level=record.levelname,
time=record.created,
message=msg)
self.records.append(entry)
except RecursionError: # See issue 36272
raise
except Exception:
self.handleError(record)
def detect(self, image):
corners, ids, _ = cv2.aruco.detectMarkers(image,
self.board.dictionary,
parameters=aruco_config(
self.aruco_params))
if ids is None: return empty_detection
_, corners, ids = cv2.aruco.interpolateCornersCharuco(
corners, ids, image, self.board)
if ids is None: return empty_detection
return struct(corners=corners.squeeze(1), ids=ids.squeeze(1))
def transformed_linear(self, camera_poses, world_points, times):
""" A linear approximation where the points are transformed at the start.
and end of the frame. Points are then computed by linear interpolation in between
"""
def transform(time_poses):
pose_table = tables.expand_views(
struct(camera=camera_poses, times=time_poses))
return tables.transform_points(
tables.expand_dims(pose_table, (2, 3)),
tables.expand_dims(world_points, (0, 1)))
start_frame = transform(self.start_table)
end_frame = transform(self.end_table)
return struct(points=lerp(start_frame.points, end_frame.points, times),
valid=start_frame.valid & end_frame.valid)
def mesh(self):
w, h = self.size
tag_offsets = np.arange(h * w).reshape(h, w) * 4
tag_quad = np.arange(0, 4).reshape(1, 4)
inner_quad = np.array([[
tag_offsets[0, 1] + 3, tag_offsets[0, 0] + 2,
tag_offsets[1, 0] + 1, tag_offsets[1, 1] + 0
]])
tag_quads = tag_quad + tag_offsets.reshape(-1, 1)
inner_quads = inner_quad + tag_offsets[:h - 1, :w - 1].reshape(-1, 1)
quads = np.concatenate([tag_quads, inner_quads])
return struct(points=self.adjusted_points,
polygons=quad_polygons(quads))
def update_image(self):
state = self.state()
layer_names, _ = self.image_layers()
layer_name = layer_names[self.layer_combo.currentIndex()]
options = struct(marker_size=self.marker_size_slider.value(),
line_width=self.line_width_slider.value(),
show_ids=self.show_ids_check.isChecked())
annotated_image = annotate_image(self.workspace,
self.calibration,
layer_name,
state,
options=options)
self.viewer_image.update_image(annotated_image)
def transformed_interpolate(self, camera_poses, world_points, times):
""" Compute in-between transforms using interpolated poses
(quanternion slerp and lerp)
"""
start_frame = np.expand_dims(self.pose_start, (0, 2, 3))
end_frame = np.expand_dims(self.pose_end, (0, 2, 3))
frame_poses = interpolate_poses(start_frame, end_frame, times)
view_poses = np.expand_dims(camera_poses.poses, (1, 2, 3)) @ frame_poses
valid = (np.expand_dims(camera_poses.valid, [1, 2, 3])
& np.expand_dims(self.valid, [0, 2, 3])
& np.expand_dims(world_points.valid, [0, 1]))
return struct(points=matrix.transform_homog(t=view_poses,
points=np.expand_dims(
world_points.points,
(0, 1))),
valid=valid)
def export(filename, calib, names, filenames, master=None):
if master is not None:
calib = calib.with_master(master)
camera_poses = calib.camera_poses.pose_table
filenames = transpose_lists(filenames)
data = struct(
cameras = export_cameras(names.camera, calib.cameras),
# camera_poses = export_sequential(names.camera, camera_poses),
camera_poses = export_camera_poses(names.camera, camera_poses)\
if master is None else export_relative(names.camera, camera_poses, master),
image_sets = export_images(names.camera, filenames)
)
with open(filename, 'w') as outfile:
json.dump(to_dicts(data), outfile, indent=2)
def find_camera_images(image_path,
cameras=None,
camera_pattern=None,
matching=True,
extensions=image.find.image_extensions):
camera_paths = image.find.find_cameras(image_path,
cameras,
camera_pattern,
extensions=extensions)
camera_names = list(camera_paths.keys())
find_images = image.find.find_images_matching if matching else image.find.find_images_unmatched
image_names, filenames = find_images(camera_paths, extensions=extensions)
info("Found camera directories {} with {} matching images".format(
camera_names, len(image_names)))
return struct(image_path=image_path,
cameras=camera_names,
image_names=image_names,
filenames=filenames)
