def convert_replica_coord(coord, rotation): rotation_replica_habitat = quat_from_two_vectors(np.array([0, 0, -1]), np.array([0, -1, 0])) obj_rotation = quaternion.from_float_array([rotation[-1]] + rotation[:-1]) obj_coord = quat_rotate_vector(rotation_replica_habitat * obj_rotation, coord) return obj_coord
def change_pos(obj_pos, obj_rot): rotation_habitat_replica = quat_from_two_vectors(np.array([0, 0, -1]), np.array([0, -1, 0])) rotation_replica_habitat = rotation_habitat_replica.inverse() obj_quat = quaternion.from_float_array(obj_rot).inverse() obj_pos = quat_rotate_vector(rotation_replica_habitat, obj_pos) obj_position_replica = quat_rotate_vector(obj_quat, obj_pos) print(obj_position_replica) return obj_position_replica
def load_connectivity(connectivity_path, scenes=None): file_format = connectivity_path + "{}_connectivity.json" if scenes: scans = scenes else: scans = read(connectivity_path + "scans.txt") connectivity = {} for scan in scans: data = read_json(file_format.format(scan)) G = load_nav_graph(data) distances = dict(nx.all_pairs_dijkstra_path_length(G)) paths = dict(nx.all_pairs_dijkstra_path(G)) positions = {} visibility = {} idxtoid = {} idtoidx = {} for i, item in enumerate(data): idxtoid[i] = item['image_id'] idtoidx[item['image_id']] = i pt_mp3d = np.array([ item['pose'][3], item['pose'][7], item['pose'][11] - item['height'] ]) q_habitat_mp3d = quat_from_two_vectors(np.array([0.0, 0.0, -1.0]), habitat_sim.geo.GRAVITY) pt_habitat = quat_rotate_vector(q_habitat_mp3d, pt_mp3d) positions[item['image_id']] = pt_habitat.tolist() visibility[item['image_id']] = { "included": item["included"], "visible": item["visible"], "unobstructed": item["unobstructed"], } connectivity[scan] = { "viewpoints": positions, "distances": distances, "paths": paths, "visibility": visibility, "idxtoid": idxtoid, "idtoidx": idtoidx, } return connectivity
def _compute_quat(self, cam_normal): """Rotations start from -z axis""" return quat_from_two_vectors(habitat_sim.geo.FRONT, cam_normal)
def _compute_quat(self, cam_normal): """Rotations start from -z axis""" return quat_from_two_vectors(np.array([0, 0, -1]), cam_normal)