def johnson(self, g : _g.Graph): ''' 根据图`g`的邻接表`adj`求最短路径对矩阵的`Johnson`算法 ''' new_g = _deepcopy(g) s = _g.Vertex('0') # V[G`] = V[G]∪{s} new_g.addvertex(s) # E[G`] = E[G]∪{(s,v),v∈V} # w(s, v) = 0 for all v∈V[G] for v in new_g.veterxs: new_g.addedgewithdir(s, v, 0) exist, weight = _sp.bellman_ford(new_g, s) if exist == False: print('the graph contains a negative-weight cycle') else: n = new_g.vertex_num D = _np.zeros((n, n)) for v in new_g.veterxs: v.d = weight for edge in new_g.edges: u, v = new_g.getvertexfromedge(edge) edge.weight = edge.weight + u.d - v.d for u in new_g.veterxs: _sp.dijstra(new_g, u) uindex = new_g.getvertexindex(u) for v in new_g.veterxs: vindex = new_g.getvertexindex(v) edge = new_g.getedge(u, v) if edge is not None: D[uindex][vindex] = edge.weight + v.d - u.d return D
def generateTree(self, name, novertex, degree=None): g = graph.Graph(name, self.directed) #Generador de arboles noedges = novertex - 1 for i in range(novertex): nv = graph.Vertex(i) g.add_vertex(nv) available = list(g.vertices) random.shuffle(available) ne = 0 leaf = [] iav = available.pop() av = g[iav] while ne < noedges: if degree is None: degree = self.getdegreevalue(noedges, novertex) for i in range(degree): # if connected if len(available) < 1 or ne >= noedges: break inv = available.pop() nv = g[inv] w = self.getweightvalue() g.add_edge(av, nv, weight=w) leaf.append(inv) ne += 1 if len(leaf) > 0: iav = random.choice(leaf) av = g[iav] leaf.remove(iav) else: av = None break return g
def walk_breadth_first(maze, start_vertex_id, target_vertex_id, max_depth=5): start_vertex = gr.Vertex(start_vertex_id) target_vertex = gr.Vertex(target_vertex_id) if start_vertex == target_vertex: print('The target and start were equal. Returning empty list') return [] #Start with first vertex and add it to the stack current_options_queue = [start_vertex] path_vertices = {} previous_vertex = start_vertex while current_options_queue: # take next vertex from the options stack current_vertex = current_options_queue[0] current_options_queue.remove(current_options_queue[0]) # get neighboring vertices that are not yet closed as dead end possible_next_steps = maze.get_neighbors(current_vertex.name) # if no more neighbors are found (except the previous edge), close the vertex if len(possible_next_steps ) <= 1 and current_vertex.name != start_vertex_id: current_vertex.state = gr.VertexState.Closed continue # if we have already investigated this vertex, skip this loop and move to the next if current_vertex.name in path_vertices.keys(): continue path_vertices[current_vertex.name] = previous_vertex previous_vertex = current_vertex # add the next possible steps to the stack for possible_step in possible_next_steps: current_options_queue.append(possible_step) # when target is part of the next steps, add the last vertex to the path and exit the loop if possible_step.name == target_vertex.name: path_vertices[target_vertex.name] = previous_vertex print('Target vertex reached !!!') # now return a list of all visited items on the path (and remove the closed ones) return track_walk_back(path_vertices, start_vertex_id, target_vertex_id) # When we arrive here, no valid path was found return None
def getGraph(): g = graph.Graph() vertices = [ 'Arad', 'Zerind', 'Timisoara', 'Sibiu', 'Oradea', 'Lugoj', 'RimnicuVilcea', 'Mehadia', 'Craiova', 'Pitesti', 'Fagaras', 'Dobreta', 'Bucharest', 'Giurgiu' ] for i in vertices: g.add_vertex(graph.Vertex(i)) edges = { 'Arad': { 'Zerind': 75, 'Sibiu': 140 }, 'Timisoara': { 'Arad': 118, 'Lugoj': 111 }, 'Sibiu': { 'Oradea': 151, 'Fagaras': 99, 'RimnicuVilcea': 80 }, 'Oradea': { 'Zerind': 71, 'Sibiu': 151 }, 'Mehadia': { 'Lugoj': 70, 'Dobreta': 75 }, 'Craiova': { 'Dobreta': 120, 'RimnicuVilcea': 146, 'Pitesti': 138 }, 'Pitesti': { 'RimnicuVilcea': 97, 'Craiova': 138 }, 'Fagaras': { 'Bucharest': 211 }, 'Dobreta': { 'Craiova': 120 }, 'Bucharest': { 'Pitesti': 101, 'Giurgiu': 90 } } for vertex1, value in edges.items(): for vertex2, cost in edges[vertex1].items(): g.add_edge(vertex1, vertex2, cost) return g
def test_bellman_ford(): g = _g.Graph() g.clear() vertexs = [_g.Vertex('s'), _g.Vertex('t'), _g.Vertex( 'x'), _g.Vertex('y'), _g.Vertex('z')] g.veterxs = vertexs g.addedgewithweight('s', 't', 6, _g.DIRECTION_TO) g.addedgewithweight('s', 'y', 7, _g.DIRECTION_TO) g.addedgewithweight('t', 'x', 5, _g.DIRECTION_TO) g.addedgewithweight('t', 'y', 8, _g.DIRECTION_TO) g.addedgewithweight('t', 'z', -4, _g.DIRECTION_TO) g.addedgewithweight('x', 't', -2, _g.DIRECTION_TO) g.addedgewithweight('y', 'x', -3, _g.DIRECTION_TO) g.addedgewithweight('y', 'z', 9, _g.DIRECTION_TO) g.addedgewithweight('z', 'x', 7, _g.DIRECTION_TO) g.addedgewithweight('z', 's', 2, _g.DIRECTION_TO) print(bellman_ford(g, vertexs[0])) del g
def test_change_key_middle_of_tree_heapify_down(self): l = [graph.Vertex(0, 3), graph.Vertex(1, 4), graph.Vertex(2, 2), graph.Vertex(3, 8), graph.Vertex(4, 9), graph.Vertex(5, 7), graph.Vertex(6, 29), graph.Vertex(7, 13)] for i in l: # lambda predicate < for a min heap self.h.insert(i) self.h.change_key(2, lambda x: x.set_data(8)) l.sort(key=lambda x: x.get_data()) for i in range(len(l)): v1 = self.h.dequeue() v2 = l[i] self.assertEqual(v1.get_data(), v2.get_data())
def test_change_key_node_has_left_child_only_heapify_up(self): l = [graph.Vertex(0, 3), graph.Vertex(1, 4), graph.Vertex(2, 2), graph.Vertex(3, 8), graph.Vertex(4, 9), graph.Vertex(5, 7), graph.Vertex(6, 29), graph.Vertex(7, 13)] for i in l: # lambda predicate < for a min heap self.h.insert(i) self.h.change_key(3, lambda x: x.set_data(1)) l.sort(key=lambda x: x.get_data()) for i in range(len(l)): v1 = self.h.dequeue() v2 = l[i] self.assertEqual(v1.get_data(), v2.get_data())
def create_maze(self, maze_edges): maze = gr.Graph() # Add vertices for all other nodes till 15 for i in range(0, 16): maze.add_vertex(gr.Vertex(i)) for edge in maze_edges: edge_parts = edge.split(':') if (len(edge_parts) == 3): # definition with distance/weight added maze.add_edge(int(edge_parts[0]), int(edge_parts[1]), int(edge_parts[2])) else: maze.add_edge(int(edge_parts[0]), int(edge_parts[1])) return maze
def test_insert(self): heapsize = 10 l = [] for i in range(heapsize): val = random.randint(1, 50) node = graph.Vertex(i, val) l.append(node) # lambda predicate < for a min heap self.h.insert(node) l.sort(key=lambda x: x.get_data()) r = [] for i in range(len(l)): v1 = self.h.dequeue() v2 = l[i] self.assertEqual(v1.get_data(), v2.get_data())
def main(): maze = gr.Graph() # Add vertices for all other nodes till 15 for i in range(0, 16): maze.add_vertex(gr.Vertex(i)) #for edge in ['02:10', '02:11', '02:01', '01:03', '03:04', '04:13', '04:05', '05:06', '05:09', '09:15', '09:07', '05:06', '06:07', '06:00', '01:00', '08:00', '08:14']: for edge in [ '1:3:1', '0:1:1', '1:2:1', '3:4:1', '4:5:1', '5:6:1', '6:8:1', '6:7:1', '6:13:1', '3:9:4', '9:10:1', '10:11:1', '10:12:1', '12:14:1', '9:13:1', '13:15:1' ]: edge_parts = edge.split(':') if (len(edge_parts) == 3): maze.add_edge(int(edge_parts[0]), int(edge_parts[1]), int(edge_parts[2])) else: maze.add_edge(int(edge_parts[0]), int(edge_parts[1])) total_distance, path = do_the_dijkstra(maze, 00, 15) print('Total distance:', total_distance) print(path)
def __init__(self, path=None, p_graph=None): if path is not None: l = open(path, 'r').readlines() #print(l) cl = l[0].split(' ') name = path.split('/')[-1].split('.')[0] if cl[0].lower == 'name': name = cl[2] i = 1 #print(name) self._graph = graph.Graph(name, directed=False) while l[i][0].isalpha(): i += 1 #print('i',i) for vl in l[i:]: il = vl.replace(' ', ' ').replace(' ', ' ').split(' ') n, vx, vy = 0, 1, 2 while il[n] == '' or il[n] == ' ': n, vx, vy = n + 1, vx + 1, vy + 1 #and il[1].isdigit() and il[2].isdigit() if len(il) >= 3 and isfloat(il[vx].replace( '\n', '')) and isfloat(il[vy].replace('\n', '')): v = graph.Vertex(il[n], coord=(float(il[vx].replace('\n', '')), float(il[vy].replace('\n', '')))) self._graph.add_vertex(v) for v in self._graph.vertices: #print(self._graph[v]) for n in self._graph.vertices: if v != n: x1, y1 = self._graph[v].label['coord'] x2, y2 = self._graph[n].label['coord'] #print(x1,y1,x2,y2) self._graph[v].add_neighbor( n, distance=math.sqrt((x2 - x1)**2 + (y2 - y1)**2)) elif graph is not None: self._graph = p_graph
def generateGraph(n, m): g = graph.Graph() if not checkiscorrectGraph(n, m): return g listvert = range(1, n + 1) for i in listvert: g.add_vertex(graph.Vertex(i)) count = 0 while count < m: r = random.sample(listvert, 2) if g.add_edge(r[0], r[1]): count = count + 1 # print(r) else: #print(str (r) + " could not be added because it was already in the graph ") continue return g
def test_dijstra(): g = _g.Graph() g.clear() vertexs = [_g.Vertex('r'), _g.Vertex('s'), _g.Vertex('t'), _g.Vertex('x'), _g.Vertex('y'), _g.Vertex('z')] g.veterxs = vertexs g.addedgewithweight('r', 's', 5, _g.DIRECTION_TO) g.addedgewithweight('s', 't', 2, _g.DIRECTION_TO) g.addedgewithweight('t', 'x', 7, _g.DIRECTION_TO) g.addedgewithweight('x', 'y', -1, _g.DIRECTION_TO) g.addedgewithweight('y', 'z', -2, _g.DIRECTION_TO) g.addedgewithweight('r', 't', 3, _g.DIRECTION_TO) g.addedgewithweight('s', 'x', 6, _g.DIRECTION_TO) g.addedgewithweight('x', 'z', 1, _g.DIRECTION_TO) g.addedgewithweight('t', 'y', 4, _g.DIRECTION_TO) g.addedgewithweight('t', 'z', 2, _g.DIRECTION_TO) g.reset_vertex_para() dijstra(g, vertexs[0]) del g
def eval(paths, m, session, starting_points_list, max_path_length=MAX_PATH_LENGTH, segment_length=SEGMENT_LENGTH, save=False, follow_targets=False, compute_targets=True, max_batch_size=model.BATCH_SIZE, window_size=WINDOW_SIZE, verbose=True, threshold_override=False): angle_losses = [] detect_losses = [] stop_losses = [] losses = [] accuracies = [] path_lengths = {path_idx: 0 for path_idx in range(len(paths))} last_time = None big_time = None for len_it in range(99999999): if len_it % 1000 == 0 and verbose: print('it {}'.format(len_it)) big_time = time.time() path_indices = [] extension_vertices = [] # get next road vertex for path_idx in range(len(paths)): # if total number of paths is greater than the maximum, then pass if path_lengths[path_idx] >= max_path_length: continue extension_vertex = paths[path_idx].pop() # if the next extension_vertex is none, then stop # in the final iter, extension vertex is none, them path_indices is none, # the program will stop if extension_vertex is None: if len(starting_points_list) > 0: # debug here to see the structure if extension_vertex st_pt = starting_points_list.pop() print("change starting point:{},{}".format( st_pt[0] - 4096, st_pt[1] - 4096)) start_point = geom.Point(st_pt[0] - 4096, st_pt[1] - 4096) extension_vertex = graph.Vertex(0, start_point) else: continue path_indices.append(path_idx) # sum the path_length path_lengths[path_idx] += 1 extension_vertices.append(extension_vertex) if len(path_indices) >= max_batch_size: break if len(path_indices) == 0: break batch_inputs = [] batch_detect_targets = [] batch_angle_targets = numpy.zeros((len(path_indices), 64), 'float32') batch_stop_targets = numpy.zeros((len(path_indices), 2), 'float32') for i in range(len(path_indices)): path_idx = path_indices[i] # 256*256*5 64*64*1 path_input, path_detect_target = model_utils.make_path_input( paths[path_idx], extension_vertices[i], segment_length, window_size=window_size) batch_inputs.append(path_input) batch_detect_targets.append(path_detect_target) if compute_targets: targets = model_utils.compute_targets_by_best( paths[path_idx], extension_vertices[i], segment_length) angle_targets, stop_targets = action_to_vector(targets) batch_angle_targets[i, :] = angle_targets batch_stop_targets[i, :] = stop_targets feed_dict = { m.is_training: False, m.inputs: batch_inputs, m.angle_targets: batch_angle_targets, m.action_targets: batch_stop_targets, m.detect_targets: batch_detect_targets, } batch_angle_outputs, batch_stop_outputs, batch_detect_outputs, angle_loss, detect_loss, stop_loss, loss = session.run( [ m.angle_outputs, m.action_outputs, m.detect_outputs, m.angle_loss, m.detect_loss, m.action_loss, m.loss ], feed_dict=feed_dict) angle_losses.append(angle_loss) detect_losses.append(detect_loss) stop_losses.append(stop_loss) losses.append(loss) batch_angle_outputs, batch_stop_outputs = fix_outputs( batch_angle_outputs, batch_stop_outputs) # whether save result if save and len_it % 1 == 0: fname = '/data/temp/{}_'.format(len_it) save_angle_targets = batch_angle_targets[0, :] if not compute_targets: save_angle_targets = None model_utils.make_path_input( paths[path_indices[0]], extension_vertices[0], segment_length, fname=fname, angle_targets=save_angle_targets, angle_outputs=batch_angle_outputs[0, :], detect_output=batch_detect_outputs[0, :, :, 0], window_size=window_size) for i in range(len(path_indices)): path_idx = path_indices[i] if len(extension_vertices[i].out_edges) >= 2: threshold = THRESHOLD_BRANCH mode = 'branch' else: threshold = THRESHOLD_FOLLOW mode = 'follow' if threshold_override: threshold = threshold_override if follow_targets == True: x = vector_to_action(batch_angle_targets[i, :], batch_stop_targets[i, :], threshold=threshold) elif follow_targets == 'partial': # (a) always use stop_targets instead of stop_outputs # (b) if we are far away from graph, use angle_targets, otherwise use angle_outputs extension_vertex = batch_extension_vertices[i] if extension_vertex.edge_pos is None or extension_vertex.edge_pos.point( ).distance(extension_vertex.point) > SEGMENT_LENGTH * 2: x = vector_to_action(batch_angle_targets[i, :], batch_stop_targets[i, :], threshold=threshold) else: x = vector_to_action(batch_angle_outputs[i, :], batch_stop_targets[i, :], threshold=threshold) elif follow_targets == 'npartial': # always move if gt says to move if batch_stop_outputs[i, 0] > threshold: x = vector_to_action(batch_angle_outputs[i, :], batch_stop_outputs[i, :], threshold=threshold) else: x = vector_to_action(batch_angle_outputs[i, :], batch_stop_targets[i, :], threshold=threshold) elif follow_targets == False: # 64, all other positions are 0 only the walk direction is p x = vector_to_action(batch_angle_outputs[i, :], batch_stop_outputs[i, :], threshold=threshold) else: raise Exception( 'invalid FOLLOW_TARGETS setting {}'.format(follow_targets)) paths[path_idx].push(extension_vertices[i], x, segment_length, training=False, branch_threshold=0.01, follow_threshold=0.01) # score accuracy accuracy = score_accuracy(batch_stop_targets[i, :], batch_angle_targets[i, :], batch_stop_outputs[i, :], batch_angle_outputs[i, :], threshold) accuracies.append(accuracy) if save: paths[0].graph.save('out.graph') return numpy.mean(angle_losses), numpy.mean(detect_losses), numpy.mean( stop_losses), numpy.mean(losses), len_it, numpy.mean(accuracies)
def as_graph(dct): """Convert a dictionary decoded from JSON into a graph. Args: dct (dict): The dictionary to convert to a graph. Returns: A graph derived from the input dictionary. """ pose_data = np.array(dct['pose_data']) if not pose_data.size: pose_data = np.zeros((0, 18)) pose_matrices = pose_data[:, :16].reshape(-1, 4, 4).transpose(0, 2, 1) odom_vertex_estimates = matrix2measurement(pose_matrices) # The camera axis used to get tag measurements are flipped # relative to the phone frame used for odom measurements camera_to_odom_transform = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # flatten the data into individual numpy arrays that we can operate on if 'tag_data' in dct: tag_pose_flat = np.vstack([[x['tagPose'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]) tag_ids = np.vstack(list(itertools.chain(*[[x['tagId'] for x in tagsFromFrame] for tagsFromFrame in \ dct['tag_data']]))) pose_ids = np.vstack(list(itertools.chain(*[[x['poseId'] for x in tagsFromFrame] for tagsFromFrame in \ dct['tag_data']]))) tag_joint_covar = np.vstack([[x['jointCovar'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]) tag_position_variances = np.vstack([[x['tagPositionVariance'] for x in tagsFromFrame] for tagsFromFrame in \ dct['tag_data']]) tag_orientation_variances = np.vstack([[x['tagOrientationVariance'] for x in tagsFromFrame] for tagsFromFrame \ in dct['tag_data']]) else: tag_pose_flat = np.zeros((0, 16)) tag_ids = np.zeros((0, 1), type=np.int) pose_ids = np.zeros((0, 1), type=np.int) tag_joint_covar = np.zeros((0, 49), type=np.double) tag_position_variances = np.zeros((0, 3), type=np.double) tag_orientation_variances = np.zeros((0, 4), type=np.double) tag_edge_measurements_matrix = np.matmul(camera_to_odom_transform, tag_pose_flat.reshape(-1, 4, 4)) tag_edge_measurements = matrix2measurement(tag_edge_measurements_matrix) # Note that we are ignoring the variance deviation of qw since we use a compact quaternion parameterization of # orientation tag_joint_covar_matrices = tag_joint_covar.reshape((-1, 7, 7)) # TODO: for some reason we have missing measurements (all zeros). Throw those out tag_edge_prescaling = np.array([np.linalg.inv(covar[:-1, :-1]) if np.linalg.det(covar[:-1, :-1]) != 0 else \ np.zeros((6, 6)) for covar in tag_joint_covar_matrices]) # print("overwriting with diagonal covariances") # tag_edge_prescaling = 1./np.hstack((tag_position_variances, tag_orientation_variances[:,:-1])) print('resetting prescaling to identity') tag_edge_prescaling = np.ones(tag_edge_prescaling.shape) unique_tag_ids = np.unique(tag_ids) tag_vertex_id_by_tag_id = dict( zip(unique_tag_ids, range(unique_tag_ids.size))) tag_id_by_tag_vertex_id = dict( zip(tag_vertex_id_by_tag_id.values(), tag_vertex_id_by_tag_id.keys())) # Enable lookup of tags by the frame they appear in tag_vertex_id_and_index_by_frame_id = {} for tag_index, (tag_id, tag_frame) in enumerate(np.hstack((tag_ids, pose_ids))): tag_vertex_id = tag_vertex_id_by_tag_id[tag_id] tag_vertex_id_and_index_by_frame_id[ tag_frame] = tag_vertex_id_and_index_by_frame_id.get( tag_frame, []) tag_vertex_id_and_index_by_frame_id[tag_frame].append( (tag_vertex_id, tag_index)) waypoint_list_uniform = list( map(lambda x: np.asarray(x[:-1]).reshape((-1, 18)), dct.get('location_data', []))) waypoint_names = list(map(lambda x: x[-1], dct.get('location_data', []))) unique_waypoint_names = np.unique(waypoint_names) if waypoint_list_uniform: waypoint_data_uniform = np.concatenate(waypoint_list_uniform) else: waypoint_data_uniform = np.zeros((0, 18)) waypoint_edge_measurements_matrix = waypoint_data_uniform[:, :16].reshape( -1, 4, 4) waypoint_edge_measurements = matrix2measurement( waypoint_edge_measurements_matrix) waypoint_vertex_id_by_name = dict( zip( unique_waypoint_names, range(unique_tag_ids.size, unique_tag_ids.size + unique_waypoint_names.size))) waypoint_name_by_vertex_id = dict( zip(waypoint_vertex_id_by_name.values(), waypoint_vertex_id_by_name.keys())) # Enable lookup of waypoints by the frame they appear in waypoint_vertex_id_and_index_by_frame_id = {} for waypoint_index, (waypoint_name, waypoint_frame) in enumerate( zip(waypoint_names, waypoint_data_uniform[:, 17])): waypoint_vertex_id = waypoint_vertex_id_by_name[waypoint_name] waypoint_vertex_id_and_index_by_frame_id[ waypoint_frame] = waypoint_vertex_id_and_index_by_frame_id.get( waypoint_name, []) waypoint_vertex_id_and_index_by_frame_id[waypoint_frame].append( (waypoint_vertex_id, waypoint_index)) # Construct the dictionaries of vertices and edges vertices = {} edges = {} vertex_counter = unique_tag_ids.size + unique_waypoint_names.size edge_counter = 0 previous_vertex = None previous_pose_matrix = None counted_tag_vertex_ids = set() counted_waypoint_vertex_ids = set() first_odom_processed = False num_tag_edges = 0 for i, odom_frame in enumerate(pose_data[:, 17]): current_odom_vertex_uid = vertex_counter vertices[current_odom_vertex_uid] = graph.Vertex( mode=graph.VertexType.ODOMETRY, estimate=odom_vertex_estimates[i], fixed=not first_odom_processed) first_odom_processed = True vertices[current_odom_vertex_uid].meta_data['poseId'] = odom_frame vertex_counter += 1 # Connect odom to tag vertex for tag_vertex_id, tag_index in tag_vertex_id_and_index_by_frame_id.get( int(odom_frame), []): if tag_vertex_id not in counted_tag_vertex_ids: vertices[tag_vertex_id] = graph.Vertex( mode=graph.VertexType.TAG, estimate=matrix2measurement(pose_matrices[i].dot( tag_edge_measurements_matrix[tag_index])), fixed=False) vertices[tag_vertex_id].meta_data[ 'tag_id'] = tag_id_by_tag_vertex_id[tag_vertex_id] counted_tag_vertex_ids.add(tag_vertex_id) edges[edge_counter] = graph.Edge( startuid=current_odom_vertex_uid, enduid=tag_vertex_id, information=np.eye(6), information_prescaling=tag_edge_prescaling[tag_index], measurement=tag_edge_measurements[tag_index], corner_ids=None, camera_intrinsics=None) num_tag_edges += 1 edge_counter += 1 # Connect odom to waypoint vertex for waypoint_vertex_id, waypoint_index in waypoint_vertex_id_and_index_by_frame_id.get( int(odom_frame), []): if waypoint_vertex_id not in counted_waypoint_vertex_ids: vertices[waypoint_vertex_id] = graph.Vertex( mode=graph.VertexType.WAYPOINT, estimate=matrix2measurement(pose_matrices[i].dot( waypoint_edge_measurements_matrix[waypoint_index])), fixed=False) vertices[waypoint_vertex_id].meta_data[ 'name'] = waypoint_name_by_vertex_id[waypoint_vertex_id] counted_waypoint_vertex_ids.add(waypoint_vertex_id) edges[edge_counter] = graph.Edge( startuid=current_odom_vertex_uid, enduid=waypoint_vertex_id, corner_ids=None, information=np.eye(6), information_prescaling=None, camera_intrinsics=None, measurement=waypoint_edge_measurements[waypoint_index]) edge_counter += 1 if previous_vertex: # TODO: might want to consider prescaling based on the magnitude of the change edges[edge_counter] = graph.Edge( startuid=previous_vertex, enduid=current_odom_vertex_uid, information=np.eye(6), information_prescaling=None, measurement=matrix2measurement( np.linalg.inv(previous_pose_matrix).dot(pose_matrices[i])), corner_ids=None, camera_intrinsics=None) edge_counter += 1 # make dummy node dummy_node_uid = vertex_counter vertices[dummy_node_uid] = graph.Vertex( mode=graph.VertexType.DUMMY, estimate=np.hstack((np.zeros(3, ), odom_vertex_estimates[i][3:])), fixed=True) vertex_counter += 1 # connect odometry to dummy node edges[edge_counter] = graph.Edge(startuid=current_odom_vertex_uid, enduid=dummy_node_uid, information=np.eye(6), information_prescaling=None, measurement=np.array( [0, 0, 0, 0, 0, 0, 1]), corner_ids=None, camera_intrinsics=None) edge_counter += 1 previous_vertex = current_odom_vertex_uid previous_pose_matrix = pose_matrices[i] resulting_graph = graph.Graph(vertices, edges, gravity_axis='y', damping_status=True) return resulting_graph
read_edges = False g = graph.Graph() vmap = {} edgelist = [] emap = {} while True: c = f.read(1) if not c: break if c == '[': uid = f.read(1) u = None if uid not in vmap: u = graph.Vertex(uid, 'oo') g.insert_vertex_object(u) vmap[uid] = u else: u = vmap[uid] if c == ':': while c != '\n': c = f.read(1) if c == '(': vid = f.read(1) v = graph.Vertex(vid, 'oo') if vid not in vmap: g.insert_vertex_object(v) vmap[vid] = v else:
def createGraph(): g = graph.Graph() l0 = graph.Vertex("0") l1 = graph.Vertex("1") l2 = graph.Vertex("2") l3 = graph.Vertex("3") l4 = graph.Vertex("4") l5 = graph.Vertex("5") l6 = graph.Vertex("6") l7 = graph.Vertex("7") l8 = graph.Vertex("8") l9 = graph.Vertex("9") l10 = graph.Vertex("10") l11 = graph.Vertex("11") l12 = graph.Vertex("12") l13 = graph.Vertex("13") l14 = graph.Vertex("14") l15 = graph.Vertex("15") l16 = graph.Vertex("16") l17 = graph.Vertex("17") l18 = graph.Vertex("18") l19 = graph.Vertex("19") l20 = graph.Vertex("20") l21 = graph.Vertex("21") l22 = graph.Vertex("22") l23 = graph.Vertex("23") l24 = graph.Vertex("24") l25 = graph.Vertex("25") l26 = graph.Vertex("26") g.addVertex(l0) g.addVertex(l1) g.addVertex(l2) g.addVertex(l3) g.addVertex(l4) g.addVertex(l5) g.addVertex(l6) g.addVertex(l7) g.addVertex(l8) g.addVertex(l9) g.addVertex(l10) g.addVertex(l11) g.addVertex(l12) g.addVertex(l13) g.addVertex(l14) g.addVertex(l15) g.addVertex(l16) g.addVertex(l17) g.addVertex(l18) g.addVertex(l19) g.addVertex(l20) g.addVertex(l21) g.addVertex(l22) g.addVertex(l23) g.addVertex(l24) g.addVertex(l25) g.addVertex(l26) g.addEdge(l0, l1, 7.2) g.addEdge(l0, l2, 3.8) g.addEdge(l0, l3, 11.0) g.addEdge(l0, l4, 2.2) g.addEdge(l0, l5, 3.5) g.addEdge(l0, l6, 10.9) g.addEdge(l0, l7, 8.6) g.addEdge(l0, l8, 7.6) g.addEdge(l0, l9, 2.8) g.addEdge(l0, l10, 6.4) g.addEdge(l0, l11, 3.2) g.addEdge(l0, l12, 7.6) g.addEdge(l0, l13, 5.2) g.addEdge(l0, l14, 4.4) g.addEdge(l0, l15, 3.7) g.addEdge(l0, l16, 7.6) g.addEdge(l0, l17, 2.0) g.addEdge(l0, l18, 3.6) g.addEdge(l0, l19, 6.5) g.addEdge(l0, l20, 1.9) g.addEdge(l0, l21, 3.4) g.addEdge(l0, l22, 2.4) g.addEdge(l0, l23, 6.4) g.addEdge(l0, l24, 2.4) g.addEdge(l0, l25, 5.0) g.addEdge(l0, l26, 3.6) g.addEdge(l1, l2, 7.1) g.addEdge(l1, l3, 6.4) g.addEdge(l1, l4, 6.0) g.addEdge(l1, l5, 4.8) g.addEdge(l1, l6, 1.6) g.addEdge(l1, l7, 2.8) g.addEdge(l1, l8, 4.8) g.addEdge(l1, l9, 6.3) g.addEdge(l1, l10, 7.3) g.addEdge(l1, l11, 5.3) g.addEdge(l1, l12, 4.8) g.addEdge(l1, l13, 3.0) g.addEdge(l1, l14, 4.6) g.addEdge(l1, l15, 4.5) g.addEdge(l1, l16, 7.4) g.addEdge(l1, l17, 6.0) g.addEdge(l1, l18, 5.0) g.addEdge(l1, l19, 4.8) g.addEdge(l1, l20, 9.5) g.addEdge(l1, l21, 10.9) g.addEdge(l1, l22, 8.3) g.addEdge(l1, l23, 6.9) g.addEdge(l1, l24, 10.0) g.addEdge(l1, l25, 4.4) g.addEdge(l1, l26, 13.0) g.addEdge(l2, l3, 9.2) g.addEdge(l2, l4, 4.4) g.addEdge(l2, l5, 2.8) g.addEdge(l2, l6, 8.6) g.addEdge(l2, l7, 6.3) g.addEdge(l2, l8, 5.3) g.addEdge(l2, l9, 1.6) g.addEdge(l2, l10, 10.4) g.addEdge(l2, l11, 3.0) g.addEdge(l2, l12, 5.3) g.addEdge(l2, l13, 6.5) g.addEdge(l2, l14, 5.6) g.addEdge(l2, l15, 5.8) g.addEdge(l2, l16, 5.7) g.addEdge(l2, l17, 4.1) g.addEdge(l2, l18, 3.6) g.addEdge(l2, l19, 4.3) g.addEdge(l2, l20, 3.3) g.addEdge(l2, l21, 5.0) g.addEdge(l2, l22, 6.1) g.addEdge(l2, l23, 9.7) g.addEdge(l2, l24, 6.1) g.addEdge(l2, l25, 2.8) g.addEdge(l2, l26, 7.4) g.addEdge(l3, l4, 5.6) g.addEdge(l3, l5, 6.9) g.addEdge(l3, l6, 8.6) g.addEdge(l3, l7, 4.0) g.addEdge(l3, l8, 11.1) g.addEdge(l3, l9, 7.3) g.addEdge(l3, l10, 1.0) g.addEdge(l3, l11, 6.4) g.addEdge(l3, l12, 11.1) g.addEdge(l3, l13, 3.9) g.addEdge(l3, l14, 4.3) g.addEdge(l3, l15, 4.4) g.addEdge(l3, l16, 7.2) g.addEdge(l3, l17, 5.3) g.addEdge(l3, l18, 6.0) g.addEdge(l3, l19, 10.6) g.addEdge(l3, l20, 5.9) g.addEdge(l3, l21, 7.4) g.addEdge(l3, l22, 4.7) g.addEdge(l3, l23, .6) g.addEdge(l3, l24, 6.4) g.addEdge(l3, l25, 10.1) g.addEdge(l3, l26, 10.1) g.addEdge(l4, l5, 1.9) g.addEdge(l4, l6, 7.9) g.addEdge(l4, l7, 5.1) g.addEdge(l4, l8, 7.5) g.addEdge(l4, l9, 2.6) g.addEdge(l4, l10, 6.5) g.addEdge(l4, l11, 1.5) g.addEdge(l4, l12, 7.5) g.addEdge(l4, l13, 3.2) g.addEdge(l4, l14, 2.4) g.addEdge(l4, l15, 2.7) g.addEdge(l4, l16, 1.4) g.addEdge(l4, l17, .5) g.addEdge(l4, l18, 1.7) g.addEdge(l4, l19, 6.5) g.addEdge(l4, l20, 3.2) g.addEdge(l4, l21, 5.2) g.addEdge(l4, l22, 2.5) g.addEdge(l4, l23, 6.0) g.addEdge(l4, l24, 4.2) g.addEdge(l4, l25, 5.4) g.addEdge(l4, l26, 5.5) g.addEdge(l5, l6, 6.3) g.addEdge(l5, l7, 4.3) g.addEdge(l5, l8, 4.5) g.addEdge(l5, l9, 1.5) g.addEdge(l5, l10, 8.7) g.addEdge(l5, l11, .8) g.addEdge(l5, l12, 4.5) g.addEdge(l5, l13, 3.9) g.addEdge(l5, l14, 3.0) g.addEdge(l5, l15, 3.8) g.addEdge(l5, l16, 5.7) g.addEdge(l5, l17, 1.9) g.addEdge(l5, l18, 1.1) g.addEdge(l5, l19, 3.5) g.addEdge(l5, l20, 4.9) g.addEdge(l5, l21, 6.9) g.addEdge(l5, l22, 4.2) g.addEdge(l5, l23, 9.0) g.addEdge(l5, l24, 5.9) g.addEdge(l5, l25, 3.5) g.addEdge(l5, l26, 7.2) g.addEdge(l6, l7, 4.0) g.addEdge(l6, l8, 4.2) g.addEdge(l6, l9, 8.0) g.addEdge(l6, l10, 8.6) g.addEdge(l6, l11, 6.9) g.addEdge(l6, l12, 4.2) g.addEdge(l6, l13, 4.2) g.addEdge(l6, l14, 8.0) g.addEdge(l6, l15, 5.8) g.addEdge(l6, l16, 7.2) g.addEdge(l6, l17, 7.7) g.addEdge(l6, l18, 6.6) g.addEdge(l6, l19, 3.2) g.addEdge(l6, l20, 11.2) g.addEdge(l6, l21, 12.7) g.addEdge(l6, l22, 10.0) g.addEdge(l6, l23, 8.2) g.addEdge(l6, l24, 11.7) g.addEdge(l6, l25, 5.1) g.addEdge(l6, l26, 14.2) g.addEdge(l7, l8, 7.7) g.addEdge(l7, l9, 9.3) g.addEdge(l7, l10, 4.6) g.addEdge(l7, l11, 4.8) g.addEdge(l7, l12, 7.7) g.addEdge(l7, l13, 1.6) g.addEdge(l7, l14, 3.3) g.addEdge(l7, l15, 3.4) g.addEdge(l7, l16, 3.1) g.addEdge(l7, l17, 5.1) g.addEdge(l7, l18, 4.6) g.addEdge(l7, l19, 6.7) g.addEdge(l7, l20, 8.1) g.addEdge(l7, l21, 10.4) g.addEdge(l7, l22, 7.8) g.addEdge(l7, l23, 4.2) g.addEdge(l7, l24, 9.5) g.addEdge(l7, l25, 6.2) g.addEdge(l7, l26, 10.7) g.addEdge(l8, l9, 4.8) g.addEdge(l8, l10, 11.9) g.addEdge(l8, l11, 4.7) g.addEdge(l8, l12, .6) g.addEdge(l8, l13, 7.6) g.addEdge(l8, l14, 7.8) g.addEdge(l8, l15, 6.6) g.addEdge(l8, l16, 7.2) g.addEdge(l8, l17, 5.9) g.addEdge(l8, l18, 5.4) g.addEdge(l8, l19, 1.0) g.addEdge(l8, l20, 8.5) g.addEdge(l8, l21, 10.3) g.addEdge(l8, l22, 7.8) g.addEdge(l8, l23, 11.5) g.addEdge(l8, l24, 9.5) g.addEdge(l8, l25, 2.8) g.addEdge(l8, l26, 14.1) g.addEdge(l9, l10, 9.4) g.addEdge(l9, l11, 1.1) g.addEdge(l9, l12, 5.1) g.addEdge(l9, l13, 4.6) g.addEdge(l9, l14, 3.7) g.addEdge(l9, l15, 4.0) g.addEdge(l9, l16, 6.7) g.addEdge(l9, l17, 2.3) g.addEdge(l9, l18, 1.8) g.addEdge(l9, l19, 4.1) g.addEdge(l9, l20, 3.8) g.addEdge(l9, l21, 5.8) g.addEdge(l9, l22, 4.3) g.addEdge(l9, l23, 7.8) g.addEdge(l9, l24, 4.8) g.addEdge(l9, l25, 3.2) g.addEdge(l9, l26, 6.0) g.addEdge(l10, l11, 7.3) g.addEdge(l10, l12, 12.0) g.addEdge(l10, l13, 4.9) g.addEdge(l10, l14, 5.2) g.addEdge(l10, l15, 5.4) g.addEdge(l10, l16, 8.1) g.addEdge(l10, l17, 6.2) g.addEdge(l10, l18, 6.9) g.addEdge(l10, l19, 11.5) g.addEdge(l10, l20, 6.9) g.addEdge(l10, l21, 8.3) g.addEdge(l10, l22, 4.1) g.addEdge(l10, l23, .4) g.addEdge(l10, l24, 4.9) g.addEdge(l10, l25, 11.0) g.addEdge(l10, l26, 6.8) g.addEdge(l11, l12, 4.7) g.addEdge(l11, l13, 3.5) g.addEdge(l11, l14, 2.6) g.addEdge(l11, l15, 2.9) g.addEdge(l11, l16, 6.3) g.addEdge(l11, l17, 1.2) g.addEdge(l11, l18, 1.0) g.addEdge(l11, l19, 3.7) g.addEdge(l11, l20, 4.1) g.addEdge(l11, l21, 6.2) g.addEdge(l11, l22, 3.4) g.addEdge(l11, l23, 6.9) g.addEdge(l11, l24, 5.2) g.addEdge(l11, l25, 3.7) g.addEdge(l11, l26, 6.4) g.addEdge(l12, l13, 7.3) g.addEdge(l12, l14, 7.8) g.addEdge(l12, l15, 6.6) g.addEdge(l12, l16, 7.2) g.addEdge(l12, l17, 5.9) g.addEdge(l12, l18, 5.4) g.addEdge(l12, l19, 1.0) g.addEdge(l12, l20, 8.5) g.addEdge(l12, l21, 10.3) g.addEdge(l12, l22, 7.8) g.addEdge(l12, l23, 11.5) g.addEdge(l12, l24, 9.5) g.addEdge(l12, l25, 2.8) g.addEdge(l12, l26, 14.1) g.addEdge(l13, l14, 1.3) g.addEdge(l13, l15, 1.5) g.addEdge(l13, l16, 4.0) g.addEdge(l13, l17, 3.2) g.addEdge(l13, l18, 3.0) g.addEdge(l13, l19, 6.9) g.addEdge(l13, l20, 6.2) g.addEdge(l13, l21, 8.2) g.addEdge(l13, l22, 5.5) g.addEdge(l13, l23, 4.4) g.addEdge(l13, l24, 7.2) g.addEdge(l13, l25, 6.4) g.addEdge(l13, l26, 10.5) g.addEdge(l14, l15, .6) g.addEdge(l14, l16, 6.4) g.addEdge(l14, l17, 2.4) g.addEdge(l14, l18, 2.2) g.addEdge(l14, l19, 6.8) g.addEdge(l14, l20, 5.3) g.addEdge(l14, l21, 7.4) g.addEdge(l14, l22, 4.6) g.addEdge(l14, l23, 4.8) g.addEdge(l14, l24, 6.3) g.addEdge(l14, l25, 6.5) g.addEdge(l14, l26, 8.8) g.addEdge(l15, l16, 5.6) g.addEdge(l15, l17, 1.6) g.addEdge(l15, l18, 1.7) g.addEdge(l15, l19, 6.4) g.addEdge(l15, l20, 4.9) g.addEdge(l15, l21, 6.9) g.addEdge(l15, l22, 4.2) g.addEdge(l15, l23, 5.6) g.addEdge(l15, l24, 5.9) g.addEdge(l15, l25, 5.7) g.addEdge(l15, l26, 8.4) g.addEdge(l16, l17, 7.1) g.addEdge(l16, l18, 6.1) g.addEdge(l16, l19, 7.2) g.addEdge(l16, l20, 10.6) g.addEdge(l16, l21, 12.0) g.addEdge(l16, l22, 9.4) g.addEdge(l16, l23, 7.5) g.addEdge(l16, l24, 11.1) g.addEdge(l16, l25, 6.2) g.addEdge(l16, l26, 13.6) g.addEdge(l17, l18, 1.6) g.addEdge(l17, l19, 4.9) g.addEdge(l17, l20, 3.0) g.addEdge(l17, l21, 5.0) g.addEdge(l17, l22, 2.3) g.addEdge(l17, l23, 5.5) g.addEdge(l17, l24, 4.0) g.addEdge(l17, l25, 5.1) g.addEdge(l17, l26, 5.2) g.addEdge(l18, l19, 4.4) g.addEdge(l18, l20, 4.6) g.addEdge(l18, l21, 6.6) g.addEdge(l18, l22, 3.9) g.addEdge(l18, l23, 6.5) g.addEdge(l18, l24, 5.6) g.addEdge(l18, l25, 4.3) g.addEdge(l18, l26, 6.9) g.addEdge(l19, l20, 7.5) g.addEdge(l19, l21, 9.3) g.addEdge(l19, l22, 6.8) g.addEdge(l19, l23, 11.4) g.addEdge(l19, l24, 8.5) g.addEdge(l19, l25, 1.8) g.addEdge(l19, l26, 13.1) g.addEdge(l20, l21, 2.0) g.addEdge(l20, l22, 2.9) g.addEdge(l20, l23, 6.4) g.addEdge(l20, l24, 2.8) g.addEdge(l20, l25, 6.0) g.addEdge(l20, l26, 4.1) g.addEdge(l21, l22, 4.4) g.addEdge(l21, l23, 7.9) g.addEdge(l21, l24, 3.4) g.addEdge(l21, l25, 7.9) g.addEdge(l21, l26, 4.7) g.addEdge(l22, l23, 4.5) g.addEdge(l22, l24, 1.7) g.addEdge(l22, l25, 6.8) g.addEdge(l22, l26, 3.1) g.addEdge(l23, l24, 5.4) g.addEdge(l23, l25, 10.6) g.addEdge(l23, l26, 7.8) g.addEdge(l24, l25, 7.0) g.addEdge(l24, l26, 1.3) g.addEdge(l25, l26, 8.3) return g
def test_type_attribute(self): node = graph.Vertex() node.type = 'Normal' self.assertEqual(node.type, 'Normal')
def test_add_neighbor(self): node = graph.Vertex() node2 = graph.Vertex() node.add_neighbor(node2) self.assertEqual(len(node.neighbors), 1)
def test_point(self): node = graph.Vertex() node.point = point.Point(coords=[0, 0]) self.assertEqual(node.point.x, 0)
def Kruskal_MBP(G, s, t): t1 = time.time() adjacency_list = G[0] weight = G[1] num_of_vertices = len(adjacency_list) edges_wt = [] di = {} for i in range(num_of_vertices): for j in range(len(adjacency_list[i])): u = i + 1 v = adjacency_list[i][j] bw = weight[u - 1][v - 1] if (u, v, bw) not in di: edges_wt.append([u, v, bw]) di[(u, v, bw)] = 1 di[(v, u, bw)] = 1 # print(edges_wt,di) MaxHeap = heap.Heap2() t2 = time.time() res = MaxHeap.HeapSort(edges_wt) edges = res[0] weights = res[1] t3 = time.time() # print(edges) # print() # print(weights) dad = [] rank = [] def MakeSet(v): dad.append(0) rank.append(0) def Find(v): r = v l = [] while dad[r] != 0: l.append(r) r = dad[r] while l: a = l.pop() dad[a] = r return r def Union(r1, r2): if rank[r1] > rank[r2]: dad[r2] = r1 elif rank[r2] > rank[r1]: dad[r1] = r2 elif rank[r2] == rank[r1]: dad[r2] = r1 rank[r1] += 1 V = [] MST_adjacency_list = [] for i in range(num_of_vertices + 1): V.append(i) MST_adjacency_list.append(g.Vertex(i)) MST = g.Graph_Kruskal() MST.add_vertices(MST_adjacency_list) for i in range(num_of_vertices + 1): MakeSet(i) for i in edges: r1 = Find(i[0]) r2 = Find(i[1]) if r1 != r2: Union(r1, r2) MST.add_edge(MST_adjacency_list[i[0]], MST_adjacency_list[i[1]]) MST_adjacency_list1 = MST.adjacency_list() # print(MST.vertices) # print(MST_adjacency_list1) MST_adjacency_list1.pop(0) t4 = time.time() #BFS to find path from s to t status = [-1 for i in range(num_of_vertices)] d = [0 for i in range(num_of_vertices)] bw = [0 for i in range(num_of_vertices)] queue = [] status[s] = 1 queue.append(s) u = s while u != t: u = queue.pop(0) for v in MST_adjacency_list1[u]: v = v - 1 # print(u,v,MST_adjacency_list1[u],queue) if status[v] == -1: status[v] = 0 d[v] = u bw[v] = weight[u][v] queue.append(v) status[u] = 1 # print(status) # print(d,bw) print_path(s, t, d, bw) t5 = time.time()
def as_graph(dct, fix_tag_vertices=False): """Convert a dictionary decoded from JSON into a graph. Args: dct (dict): The dictionary to convert to a graph. Returns: A graph derived from the input dictionary. """ pose_data = np.array(dct['pose_data']) if not pose_data.size: pose_data = np.zeros((0, 18)) pose_matrices = pose_data[:, :16].reshape(-1, 4, 4).transpose(0, 2, 1) odom_vertex_estimates = matrix2measurement(pose_matrices, invert=True) tag_size = 0.173 # TODO: need to send this with the tag detection true_3d_points = np.array([[-tag_size / 2, -tag_size / 2, 1], [tag_size / 2, -tag_size / 2, 1], [tag_size / 2, tag_size / 2, 1], [-tag_size / 2, tag_size / 2, 1]]) true_3d_tag_center = np.array([0, 0, 1]) # The camera axis used to get tag measurements are flipped # relative to the phone frame used for odom measurements camera_to_odom_transform = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # flatten the data into individual numpy arrays that we can operate on if 'tag_data' in dct and len(dct['tag_data']) > 0: tag_pose_flat = np.vstack([[x['tagPose'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]) camera_intrinsics_for_tag = np.vstack( [[x['cameraIntrinsics'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]) tag_corners = np.vstack( [[x['tagCornersPixelCoordinates'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]) tag_ids = np.vstack( list( itertools.chain(*[[x['tagId'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]))) pose_ids = np.vstack( list( itertools.chain(*[[x['poseId'] for x in tagsFromFrame] for tagsFromFrame in dct['tag_data']]))) else: tag_pose_flat = np.zeros((0, 16)) camera_intrinsics_for_tag = np.zeros((0, 4)) tag_corners = np.zeros((0, 8)) tag_ids = np.zeros((0, 1), dtype=np.int) pose_ids = np.zeros((0, 1), dtype=np.int) tag_edge_measurements_matrix = np.matmul(camera_to_odom_transform, tag_pose_flat.reshape(-1, 4, 4)) tag_edge_measurements = matrix2measurement(tag_edge_measurements_matrix) # Note that we are ignoring the standard deviation of qw since we use a compact quaternion parameterization of orientation unique_tag_ids = np.unique(tag_ids) tag_vertex_id_by_tag_id = dict( zip(unique_tag_ids, range(0, unique_tag_ids.size * 5, 5))) tag_id_by_tag_vertex_id = dict( zip(tag_vertex_id_by_tag_id.values(), tag_vertex_id_by_tag_id.keys())) tag_corner_ids_by_tag_vertex_id = dict( zip( tag_id_by_tag_vertex_id.keys(), map( lambda tag_vertex_id: list( range(tag_vertex_id + 1, tag_vertex_id + 5)), tag_id_by_tag_vertex_id.keys()))) # Enable lookup of tags by the frame they appear in tag_vertex_id_and_index_by_frame_id = {} for tag_index, (tag_id, tag_frame) in enumerate(np.hstack((tag_ids, pose_ids))): tag_vertex_id = tag_vertex_id_by_tag_id[tag_id] tag_vertex_id_and_index_by_frame_id[ tag_frame] = tag_vertex_id_and_index_by_frame_id.get( tag_frame, []) tag_vertex_id_and_index_by_frame_id[tag_frame].append( (tag_vertex_id, tag_index)) waypoint_list_uniform = list( map(lambda x: np.asarray(x[:-1]).reshape((-1, 18)), dct.get('location_data', []))) waypoint_names = list(map(lambda x: x[-1], dct.get('location_data', []))) unique_waypoint_names = np.unique(waypoint_names) if waypoint_list_uniform: waypoint_data_uniform = np.concatenate(waypoint_list_uniform) else: waypoint_data_uniform = np.zeros((0, 18)) waypoint_edge_measurements_matrix = waypoint_data_uniform[:, :16].reshape( -1, 4, 4) waypoint_edge_measurements = matrix2measurement( waypoint_edge_measurements_matrix) waypoint_vertex_id_by_name = dict( zip( unique_waypoint_names, range(unique_tag_ids.size * 5, unique_tag_ids.size * 5 + unique_waypoint_names.size))) waypoint_name_by_vertex_id = dict( zip(waypoint_vertex_id_by_name.values(), waypoint_vertex_id_by_name.keys())) # Enable lookup of waypoints by the frame they appear in waypoint_vertex_id_and_index_by_frame_id = {} for waypoint_index, (waypoint_name, waypoint_frame) in enumerate( zip(waypoint_names, waypoint_data_uniform[:, 17])): waypoint_vertex_id = waypoint_vertex_id_by_name[waypoint_name] waypoint_vertex_id_and_index_by_frame_id[ waypoint_frame] = waypoint_vertex_id_and_index_by_frame_id.get( waypoint_name, []) waypoint_vertex_id_and_index_by_frame_id[waypoint_frame].append( (waypoint_vertex_id, waypoint_index)) # Construct the dictionaries of vertices and edges vertices = {} edges = {} vertex_counter = unique_tag_ids.size * 5 + unique_waypoint_names.size edge_counter = 0 previous_vertex = None counted_tag_vertex_ids = set() counted_waypoint_vertex_ids = set() first_odom_processed = False num_tag_edges = 0 # DEBUG: this appears to be counterproductive initialize_with_averages = False tag_transform_estimates = defaultdict(lambda: []) for i, odom_frame in enumerate(pose_data[:, 17]): current_odom_vertex_uid = vertex_counter vertices[current_odom_vertex_uid] = graph.Vertex( mode=graph.VertexType.ODOMETRY, estimate=odom_vertex_estimates[i], fixed=not first_odom_processed) vertices[current_odom_vertex_uid].meta_data['poseId'] = odom_frame first_odom_processed = True vertex_counter += 1 # Connect odom to tag vertex for tag_vertex_id, tag_index in tag_vertex_id_and_index_by_frame_id.get( int(odom_frame), []): current_tag_transform_estimate = SE3Quat( np.hstack((true_3d_tag_center, [0, 0, 0, 1]))) * SE3Quat( tag_edge_measurements[tag_index]).inverse() * SE3Quat( vertices[current_odom_vertex_uid].estimate) # keep track of estimates in case we want to average them to initialize the graph tag_transform_estimates[tag_vertex_id].append( current_tag_transform_estimate) if tag_vertex_id not in counted_tag_vertex_ids: vertices[tag_vertex_id] = graph.Vertex( mode=graph.VertexType.TAG, estimate=current_tag_transform_estimate.to_vector(), fixed=fix_tag_vertices) vertices[tag_vertex_id].meta_data[ 'tag_id'] = tag_id_by_tag_vertex_id[tag_vertex_id] for idx, true_point_3d in enumerate(true_3d_points): vertices[tag_corner_ids_by_tag_vertex_id[tag_vertex_id] [idx]] = graph.Vertex( mode=graph.VertexType.TAGPOINT, estimate=np.hstack( (true_point_3d, [0, 0, 0, 1])), fixed=True) counted_tag_vertex_ids.add(tag_vertex_id) # adjust the x-coordinates of the detections to account for differences in coordinate systems induced by the camera_to_odom_transform tag_corners[tag_index][::2] = 2 * camera_intrinsics_for_tag[ tag_index][2] - tag_corners[tag_index][::2] # TODO: create proper subclasses for k, point in enumerate(true_3d_points): point_in_camera_frame = SE3Quat( tag_edge_measurements[tag_index]) * (point - np.array([0, 0, 1])) cam = CameraParameters( camera_intrinsics_for_tag[tag_index][0], camera_intrinsics_for_tag[tag_index][2:], 0) #print("chi2", np.sum(np.square(tag_corners[tag_index][2*k : 2*k + 2] - cam.cam_map(point_in_camera_frame)))) edges[edge_counter] = graph.Edge( startuid=current_odom_vertex_uid, enduid=tag_vertex_id, corner_ids=tag_corner_ids_by_tag_vertex_id[tag_vertex_id], information=np.eye(2), information_prescaling=None, camera_intrinsics=camera_intrinsics_for_tag[tag_index], measurement=tag_corners[tag_index]) num_tag_edges += 1 edge_counter += 1 # Connect odom to waypoint vertex for waypoint_vertex_id, waypoint_index in waypoint_vertex_id_and_index_by_frame_id.get( int(odom_frame), []): if waypoint_vertex_id not in counted_waypoint_vertex_ids: vertices[waypoint_vertex_id] = graph.Vertex( mode=graph.VertexType.WAYPOINT, estimate=(SE3Quat( vertices[current_odom_vertex_uid].estimate).inverse() * SE3Quat( waypoint_edge_measurements[waypoint_index]) ).to_vector(), fixed=False) vertices[waypoint_vertex_id].meta_data[ 'name'] = waypoint_name_by_vertex_id[waypoint_vertex_id] counted_waypoint_vertex_ids.add(waypoint_vertex_id) edges[edge_counter] = graph.Edge( startuid=current_odom_vertex_uid, enduid=waypoint_vertex_id, corner_ids=None, information=np.eye(6), information_prescaling=None, camera_intrinsics=None, measurement=( SE3Quat(vertices[waypoint_vertex_id].estimate) * SE3Quat( vertices[current_odom_vertex_uid].estimate).inverse() ).to_vector()) edge_counter += 1 if previous_vertex: # TODO: might want to consider prescaling based on the magnitude of the change edges[edge_counter] = graph.Edge( startuid=previous_vertex, enduid=current_odom_vertex_uid, corner_ids=None, information=np.eye(6), information_prescaling=None, camera_intrinsics=None, measurement=( SE3Quat(vertices[current_odom_vertex_uid].estimate) * SE3Quat(vertices[previous_vertex].estimate).inverse() ).to_vector()) edge_counter += 1 dummy_node_uid = vertex_counter vertices[dummy_node_uid] = graph.Vertex( mode=graph.VertexType.DUMMY, estimate=np.hstack((np.zeros(3, ), odom_vertex_estimates[i][3:])), fixed=True) vertex_counter += 1 edges[edge_counter] = graph.Edge(startuid=current_odom_vertex_uid, enduid=dummy_node_uid, corner_ids=None, information=np.eye(6), information_prescaling=None, camera_intrinsics=None, measurement=np.array( [0, 0, 0, 0, 0, 0, 1])) edge_counter += 1 previous_vertex = current_odom_vertex_uid if initialize_with_averages: for vertex_id, transforms in tag_transform_estimates.items(): vertices[vertex_id].estimate = se3_quat_average( transforms).to_vector() # TODO: Huber delta should probably scale with pixels rather than error resulting_graph = graph.Graph(vertices, edges, gravity_axis='y', is_sparse_bundle_adjustment=True, use_huber=False, huber_delta=None, damping_status=True) return resulting_graph
def buildGraph(self): ''' 练习22.2-1 练习22.2-2 ''' g = _g.Graph() g.veterxs = [_g.Vertex('1'), _g.Vertex('2'), _g.Vertex('3'), _g.Vertex('4'), _g.Vertex('5'), _g.Vertex('6')] g.edges.clear() g.edges.append(_g.Edge(_g.Vertex('1'), _g.Vertex('2'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('4'), _g.Vertex('2'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('1'), _g.Vertex('4'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('2'), _g.Vertex('5'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('3'), _g.Vertex('6'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('3'), _g.Vertex('5'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('5'), _g.Vertex('4'), 1, _g.DIRECTION_TO)) g.edges.append(_g.Edge(_g.Vertex('6'), _g.Vertex('6'), 1, _g.DIRECTION_TO)) _g.bfs(g, g.veterxs[2]) _g.print_path(g, g.veterxs[2], g.veterxs[4]) print('') del g g = _g.Graph() g.veterxs.clear() g.edges.clear() v = ['r', 's', 't', 'u', 'v', 'w', 'x', 'y'] g.addvertex(v) g.addedge('v', 'r') g.addedge('r', 's') g.addedge('s', 'w') g.addedge('w', 'x') g.addedge('w', 't') g.addedge('x', 't') g.addedge('x', 'u') g.addedge('x', 'y') g.addedge('y', 'u') g.addedge('u', 't') _g.bfs(g, 'u') _g.print_path(g, 'u', 'v') print('') del g
# TEST GRAPH SCRIPT WITH SIMPLE EXAMPLE # Santiago Garcia Arango, July 2020 import graph # Create Graph object g1 = graph.Graph() # Create all Vertex objects (that will be added to Graph) S = graph.Vertex("S") A = graph.Vertex("A") B = graph.Vertex("B") C = graph.Vertex("C") D = graph.Vertex("D") E = graph.Vertex("E") G = graph.Vertex("G") # Add all Vertex objects to the graph g1.add_vertex(S) g1.add_vertex(A) g1.add_vertex(B) g1.add_vertex(C) g1.add_vertex(D) g1.add_vertex(E) g1.add_vertex(G) g1.add_edge(S, A, 3) g1.add_edge(S, B, 5) g1.add_edge(A, B, 4) g1.add_edge(B, C, 4) g1.add_edge(C, E, 7)
def make_vertex(pos): v = graph.Vertex() data.main_graph.add_vertex(v) vd = graphics.VertexDrawable(v.id, pos) data.drawable_vertices[v.id] = vd
def test_change_key_list_of_one(self): v = graph.Vertex(0, 3) self.h.insert(v) self.h.change_key(0, lambda x: x.set_data(32)) x = self.h.dequeue() self.assertEqual(graph.Vertex(0, 32), x)