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)