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()