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)