def get_transform(self, vertex_id1, vertex_id2):
"""Returns the transform, T_21, from vertex 1 to vertex 2 in the graph.
Args:
vertex_id1 (tuple of ints): Id of the first vertex object. The id is a tuple
containing the run id and the the pose id.
vertex_id2 (tuple of ints): Id of the second vertex object. The id is a tuple
containing the run id and the the pose id.
Returns:
Transform: The transform, T_21, from vertex 1 to vertex 2. Identity transform if no
path exists between the vertices.
"""
transform = Transform(np.eye(3), np.zeros((3, )))
path, forward = self.get_path(vertex_id1, vertex_id2)
if len(path) == 0:
print(
"Warning: no path found between vertex {0} and vertex {1}, returning identity transform."
.format(vertex_id1, vertex_id2))
return transform
if len(path) == 1:
return transform
# Always calculate transform in forward direction then flip if backward
if not forward:
path.reverse()
# Compose transforms
for vertex in path[:-2]:
transform = self.get_vertex(vertex).next_transform * transform
# Check if last vertex in path is a teach vertex
if self.get_vertex(path[-2]).next_id == path[-1]:
transform = self.get_vertex(path[-2]).next_transform * transform
else:
transform = self.get_vertex(
path[-1]).next_transform.inv() * transform
if forward:
return transform
else:
return transform.inv()
