def _get_frame_id_to_extrinsics_init( camera_intrinsics, key_markers, frame_id_to_extrinsics_init, marker_id_to_extrinsics_init, frame_ids, ): """ calculate camera extrinsics based on the known marker extrinsics """ frame_ids_not_computed = set(frame_ids) - set(frame_id_to_extrinsics_init.keys()) for frame_id in frame_ids_not_computed: markers_in_frame = [ {"id": marker.marker_id, "verts": marker.verts} for marker in key_markers if marker.frame_id == frame_id and marker.marker_id in marker_id_to_extrinsics_init.keys() ] camera_extrinsics = solvepnp.calculate( camera_intrinsics, markers_in_frame, marker_id_to_extrinsics_init ) if camera_extrinsics is not None: frame_id_to_extrinsics_init[frame_id] = camera_extrinsics return frame_id_to_extrinsics_init
def _get_frame_id_to_extrinsics_init( camera_intrinsics, key_markers, frame_id_to_extrinsics_init, marker_id_to_extrinsics_init, frame_ids, ): """calculate camera extrinsics based on the known marker extrinsics""" frame_ids_not_computed = set(frame_ids) - set( frame_id_to_extrinsics_init.keys()) for frame_id in frame_ids_not_computed: markers_in_frame = [ { "id": marker.marker_id, "verts": marker.verts } for marker in key_markers if marker.frame_id == frame_id and marker.marker_id in marker_id_to_extrinsics_init.keys() ] camera_extrinsics = solvepnp.calculate(camera_intrinsics, markers_in_frame, marker_id_to_extrinsics_init) if camera_extrinsics is not None: frame_id_to_extrinsics_init[frame_id] = camera_extrinsics return frame_id_to_extrinsics_init
def offline_localization( timestamps, frame_index_range, markers_bisector, frame_index_to_num_markers, marker_id_to_extrinsics, camera_intrinsics, shared_memory, ): batch_size = 300 def find_markers_in_frame(index): window = pm.enclosing_window(timestamps, index) return markers_bisector.by_ts_window(window) camera_extrinsics_prv = None not_localized_count = 0 frame_start, frame_end = frame_index_range frame_count = frame_end - frame_start + 1 frame_indices = sorted( set(range(frame_start, frame_end + 1)) & set(frame_index_to_num_markers.keys())) queue = [] for frame_index in frame_indices: shared_memory.progress = (frame_index - frame_start + 1) / frame_count if frame_index_to_num_markers[frame_index]: markers_in_frame = find_markers_in_frame(frame_index) camera_extrinsics = solvepnp.calculate( camera_intrinsics, markers_in_frame, marker_id_to_extrinsics, camera_extrinsics_prv=camera_extrinsics_prv, min_n_markers_per_frame=1, ) if camera_extrinsics is not None: camera_extrinsics_prv = camera_extrinsics not_localized_count = 0 timestamp = timestamps[frame_index] pose_data = get_pose_data(camera_extrinsics, timestamp) serialized_dict = fm.Serialized_Dict(pose_data) queue.append((timestamp, serialized_dict)) if len(queue) >= batch_size: data = queue[:batch_size] del queue[:batch_size] yield data continue not_localized_count += 1 if not_localized_count >= 5: camera_extrinsics_prv = None yield queue
def offline_localization( timestamps, frame_index_range, markers_bisector, frame_index_to_num_markers, marker_id_to_extrinsics, camera_intrinsics, shared_memory, ): batch_size = 300 def find_markers_in_frame(index): window = pm.enclosing_window(timestamps, index) return markers_bisector.by_ts_window(window) camera_extrinsics_prv = None not_localized_count = 0 frame_start, frame_end = frame_index_range frame_count = frame_end - frame_start + 1 frame_indices = sorted( set(range(frame_start, frame_end + 1)) & set(frame_index_to_num_markers.keys()) ) queue = [] for frame_index in frame_indices: shared_memory.progress = (frame_index - frame_start + 1) / frame_count if frame_index_to_num_markers[frame_index]: markers_in_frame = find_markers_in_frame(frame_index) camera_extrinsics = solvepnp.calculate( camera_intrinsics, markers_in_frame, marker_id_to_extrinsics, camera_extrinsics_prv=camera_extrinsics_prv, min_n_markers_per_frame=1, ) if camera_extrinsics is not None: camera_extrinsics_prv = camera_extrinsics not_localized_count = 0 timestamp = timestamps[frame_index] pose_data = get_pose_data(camera_extrinsics, timestamp) serialized_dict = fm.Serialized_Dict(pose_data) queue.append((timestamp, serialized_dict)) if len(queue) >= batch_size: data = queue[:batch_size] del queue[:batch_size] yield data continue not_localized_count += 1 if not_localized_count >= 5: camera_extrinsics_prv = None yield queue
def online_localization( timestamp, detection_storage, optimization_storage, localization_storage, camera_intrinsics, ): camera_extrinsics = solvepnp.calculate( camera_intrinsics, detection_storage.current_markers, optimization_storage.marker_id_to_extrinsics, localization_storage.current_pose["camera_extrinsics"], min_n_markers_per_frame=1, ) return get_pose_data(camera_extrinsics, timestamp)
def online_localization( timestamp, detection_storage, optimization_storage, localization_storage, camera_intrinsics, ): camera_extrinsics = solvepnp.calculate( camera_intrinsics, detection_storage.current_markers, optimization_storage.marker_id_to_extrinsics, localization_storage.current_pose["camera_extrinsics"], min_n_markers_per_frame=1, ) return get_pose_data(camera_extrinsics, timestamp)