def create_boxed_graph(size): g = Graph() for y in range(size): for x in range(size): current_node = (x, y) for displacement_vector in neighbour_displacements: neighbour_node = add_vector(displacement_vector, current_node, 2) if -1 not in neighbour_node: g.add_edge( current_node, neighbour_node, get_distance_from_neighbour_vector( displacement_vector)) return g
class TestGraphMethods(TestCase): def setUp(self): self.__graph = Graph() def test_structure(self): self.assertIsInstance(self.__graph, Graph) def test_add_node(self): self.assertEqual(len(self.__graph.get_all_node()), 0) self.assertNotEqual(len(self.__graph.get_all_node()), 1) self.__graph.add_node(node=1) self.assertNotEqual(len(self.__graph.get_all_node()), 0) self.assertEqual(len(self.__graph.get_all_node()), 1) def test_add_edges(self): # Adding nodes self.__graph.add_node(node='B') self.__graph.add_node(node='A') # Connecting two nodes self.__graph.add_edge(edge=('A', 'B')) # Adding nodes self.__graph.add_node(node='C') self.__graph.add_node(node='D') self.__graph.add_edge(edge=('A', 'D')) # Assertions self.assertTrue(self.__graph.is_connected('A', 'B')) self.assertTrue(self.__graph.is_connected('B', 'A')) self.assertFalse(self.__graph.is_connected('C', 'D')) self.assertFalse(self.__graph.is_connected('B', 'D')) with self.assertRaises(TypeError): self.__graph.add_edge(edge=('Z', 'F')) def test_if_two_nodes_was_on_the_same_network(self): # Adding nodes self.__graph.add_node(node='C') self.__graph.add_node(node='D') self.__graph.add_node(node='A') self.__graph.add_node(node='B') self.__graph.add_edge(edge=('A', 'D')) # Assertions self.assertFalse(self.__graph.same_network(('A', 'B'))) self.assertFalse(self.__graph.same_network(('B', 'A'))) self.assertFalse(self.__graph.same_network(('C', 'D'))) self.assertFalse(self.__graph.same_network(('B', 'D'))) self.assertTrue(self.__graph.same_network(('A', 'D'))) self.assertTrue(self.__graph.same_network(('D', 'A')))
def updateRoutingTable(self): """ Update routing table using distributed Djistra algorithm. """ g = Graph() networks = {} # print(self.sw.name, self.lsdb) for rid, lsa in self.lsdb.items(): for neigh in lsa['networks']: # rid, neigh[2] subnet, netmask, neighid = neigh g.add_edge(rid, neighid) netaddr = ipprefix(subnet, netmask) if netaddr not in networks: networks[netaddr] = set() networks[netaddr].add(rid) # print(self.sw.name, g.adj) # print(self.sw.name, networks) next_hops = g.find_shortest_paths(self.sw.router_id) # print(self.sw.name, next_hops) for netaddr, nodes in networks.items(): if len(nodes) == 1: dst = nodes.pop() if dst == self.sw.router_id: nhop = None else: nhop, _ = next_hops.get(dst, (None, None)) elif len(nodes) == 2: n1, n2 = nodes if self.sw.router_id in nodes: dst = nhop = (n2 if n1 == self.sw.router_id else n1) else: dst = (n1 if next_hops[n1][1] < next_hops[n2][1] else n2) nhop, _ = next_hops[dst] for pn, p in self.sw.data_ports.items(): gateway = p.ownNeigh(nhop) if ipprefix(p.IP(), p.Netmask()) == netaddr: gateway = '0.0.0.0' if gateway is not None: r = (netaddr, gateway, pn) self.sw.pending_pwospf_table[netaddr] = r
# parse puzzle input into a graph for line in puzzle_input: program_name = line.split()[0] programs.add(program_name) weight = get_numbers(line)[0] n = get_or_create_node(program_name, weight) if '-> ' not in line: continue sline = line.split('-> ')[1].split(', ') for p in sline: child_node = get_or_create_node(p) e = Edge(n,child_node) g.add_edge(e) targets.add(p) # start at the bottom until we find the first node that isn't misbalanced # to balance everything out, we have to change the parent node of this node bottom = programs.difference(targets).pop() current_node = get_or_create_node(bottom) balancer = None print('Part 1:', bottom) while current_node is not None: wrong_part = {} for neighbour in g.get_neighbours(current_node):
selection = "" graph.add_vertex("A") graph.add_vertex("A#/Bb") graph.add_vertex("B") graph.add_vertex("C") graph.add_vertex("C#/Db") graph.add_vertex("D") graph.add_vertex("D#/Eb") graph.add_vertex("E") graph.add_vertex("F") graph.add_vertex("F#/Gb") graph.add_vertex("G") graph.add_vertex("G#/Ab") graph.add_edge("A", "A#/Bb") graph.add_edge("A#/Bb", "B") graph.add_edge("B", "C") graph.add_edge("C", "C#/Db") graph.add_edge("C#/Db", "D") graph.add_edge("D", "D#/Eb") graph.add_edge("D#/Eb", "E") graph.add_edge("E", "F") graph.add_edge("F", "F#/Gb") graph.add_edge("F#/Gb", "G") graph.add_edge("G", "G#/Ab") graph.add_edge("G#/Ab", "A") root_note = [{ "type": "list",
def estimate_extrinsics_pnp(tagpose_estimator, cam_intrinsic, cam_dist, point2d_coord, point2d_cid, point2d_fid, point2d_pid, point2d_mid, verbose=0): """ Estimates extrinsic parameters for each camera from the given 2D point correspondences alone. It estimates the essential matrix for camera pairs along the observation graph. Input: tagpose_estimator: custom object, Estimates the pose between a camera and the calibration objects. cam_intrinsic: list of 3x3 np.array, Intrinsic calibration of each camera. cam_dist: list of 1x5 np.array, Distortion coefficients following the OpenCV pinhole camera model. point2d_coord: Nx2 np.array, Array containing 2D coordinates of N points. point2d_cid: Nx1 np.array, Array containing the camera id for each of the N points. point2d_fid: Nx1 np.array, Array containing the frame id for each of the N points. point2d_pid: Nx1 np.array, Array containing a unique point id for each of the N points. point2d_mid: Nx1 np.array, Array containing a marker-unique id for each of the N points. Returns: cam_extrinsic: list of 4x4 np.array, Intrinsic calibration of each camera. calib_object_points3d: Mx3 np.array, 3D Points of the calibration object in a object based frame. """ assert len(cam_intrinsic) >= 2, "Too little cameras." assert len(cam_intrinsic) == len(cam_dist), "Shape mismatch." assert len(point2d_cid.shape) == 1, "Shape mismatch." assert len(point2d_fid.shape) == 1, "Shape mismatch." assert len(point2d_pid.shape) == 1, "Shape mismatch." assert len(point2d_mid.shape) == 1, "Shape mismatch." assert point2d_coord.shape[0] == point2d_cid.shape[0], "Shape mismatch." assert point2d_coord.shape[0] == point2d_fid.shape[0], "Shape mismatch." assert point2d_coord.shape[0] == point2d_pid.shape[0], "Shape mismatch." assert point2d_coord.shape[0] == point2d_mid.shape[0], "Shape mismatch." assert len(cam_intrinsic) == len( np.unique(point2d_cid).flatten().tolist()), "Shape mismatch." if verbose > 0: print('\n\n------------') print('- Estimating extrinsic parameters by solving PNP problems') num_cams = len(cam_intrinsic) num_frames = np.max(point2d_fid) + 1 # get model shape calib_object_points3d = tagpose_estimator.object_points.copy() # 1. Iterate cams and estimate relative pose to the calibration object for each frame scores_object, T_obj2cam = estimate_and_score_object_poses( tagpose_estimator, point2d_coord, point2d_cid, point2d_fid, point2d_mid, cam_intrinsic, cam_dist) def _calc_score(T1, T2): """ Estimates how closely T1 * T2 = eye() holds. """ R = np.matmul(T1, T2) - np.eye(T1.shape[0]) return np.sum(np.abs(R)) # frobenius norm # try to find the pair of frames which worked best --> estimate relative camera pose from there scores_rel_calib = dict( ) # store how good this guess seems to be for calibrating a cam pair for fid1 in range(num_frames): # for each pair of frames for fid2 in range(fid1, num_frames): scores_rel_calib[fid1, fid2] = dict() for cid1 in range(num_cams): # check each pair of cams for cid2 in range(cid1 + 1, num_cams): # check if its valid if (T_obj2cam[fid1][cid2] is None) or (T_obj2cam[fid1][cid1] is None) or \ (T_obj2cam[fid2][cid2] is None) or (T_obj2cam[fid2][cid1] is None): s_rel = float('inf') else: # calculate the transformation cam1 -> cams2 using fid1 T12_fid1 = np.matmul( T_obj2cam[fid1][cid2], np.linalg.inv(T_obj2cam[fid1][cid1])) # calculate the transformation cam2 -> cams1 using fid2 T21_fid2 = np.matmul( T_obj2cam[fid2][cid1], np.linalg.inv(T_obj2cam[fid2][cid2])) # for perfect estimations the two mappings should be the inverse of each others s_rel = _calc_score(T12_fid1, T21_fid2) scores_rel_calib[fid1, fid2][cid1, cid2] = s_rel # 3. Find out which frames are optimal for a given cam pair cam_pair_best_fid = dict() for cid1 in range(num_cams): for cid2 in range(cid1 + 1, num_cams): min_fid = None min_v = float('inf') for fid_pair, score_dict in scores_rel_calib.items(): # get an initial value if min_fid is None: min_fid = fid_pair min_v = score_dict[cid1, cid2] continue # if current best is worse than current item replace if min_v > score_dict[cid1, cid2]: min_fid = fid_pair min_v = score_dict[cid1, cid2] cam_pair_best_fid[cid1, cid2] = min_fid # 3. Build observation graph and use djikstra to estimate relative camera poses observation_graph = Graph() for cid in range(num_cams): observation_graph.add_node(cid) # populate with edges score_accumulated = [0 for _ in range(num_cams) ] # accumulate score for each cam for cid1 in range(num_cams): for cid2 in range(cid1 + 1, num_cams): fid_pair = cam_pair_best_fid[cid1, cid2] s = scores_rel_calib[fid_pair][cid1, cid2] observation_graph.add_edge(cid1, cid2, s) observation_graph.add_edge(cid2, cid1, s) score_accumulated[cid1] += s score_accumulated[cid2] += s # root cam (the one that has the lowest overall score) root_cam_id = np.argmin(np.array(score_accumulated)) if verbose > 1: print('- Accumulated score (lower is better): ', score_accumulated) print('- Choosing root cam', root_cam_id) # 4. Determine which relative poses to estimate # use Dijkstra to find "cheapest" path (i.e. the one with most observations) from the starting cam to all others cam_path = dict( ) # contains how to get from the starting cam to another one cam_path[target_cam] = [path] for cid in range(num_cams): if cid == root_cam_id: cam_path[cid] = [cid] continue cost, camchain = shortest_path(observation_graph, root_cam_id, cid) cam_path[cid] = camchain if verbose > 1: for k, v in cam_path.items(): print('- Camchain to %d: ' % k, v) # 5. Put together the relative camera poses relative_pose = dict() # contains the trafo from start -> target relative_pose_pair = dict( ) # contains already estimated poses between cameras; # is the trafo from j -> i; xi = relative_pose_pair[i, j] * xj for target_camid in range(num_cams): if target_camid == root_cam_id: relative_pose[target_camid] = np.eye(4) continue M = np.eye(4) # this is the trafo from start -> X for i in range(len(cam_path[target_camid]) - 1): # traverse cam path # get current cam_pair on the cam_path inter_camid1 = cam_path[target_camid][ i] # this is where we currently are inter_camid2 = cam_path[target_camid][ i + 1] # this is where we want to transform to swapped = False if inter_camid2 < inter_camid1: t = inter_camid2 inter_camid2 = inter_camid1 inter_camid1 = t swapped = True if verbose > 1: print( '- Attempting to estimate the relative pose from cam %d --> %d' % (inter_camid1, inter_camid2)) # calculate only when not calculated yet if (inter_camid1, inter_camid2) not in relative_pose_pair.keys(): fid1, fid2 = cam_pair_best_fid[inter_camid1, inter_camid2] msg = "Calibration impossible! There is no way feasible way to calibrate cam%d and cam%d." % \ (inter_camid1, inter_camid2) assert T_obj2cam[fid1][inter_camid1] is not None, msg assert T_obj2cam[fid1][inter_camid2] is not None, msg # calculate the transformation cam1 -> cams2 using the optimal fids T12 = np.matmul(T_obj2cam[fid1][inter_camid1], np.linalg.inv(T_obj2cam[fid1][inter_camid2])) relative_pose_pair[inter_camid1, inter_camid2] = T12 delta = relative_pose_pair[inter_camid1, inter_camid2] if swapped: delta = np.linalg.inv(delta) # accumulate trafos M = np.matmul(delta, M) relative_pose[target_camid] = M if verbose > 0: print('- Extrinsics estimated') if verbose > 2: for cid in range(num_cams): print('\n- Trafo Root (%d) --> %d' % (root_cam_id, cid)) print(relative_pose[cid]) print('') cam_extrinsic = list() for cid in range(num_cams): cam_extrinsic.append(relative_pose[cid]) # 6. Figure out the object poses (if there is no observation its impossible) object_poses = greedy_pick_object_pose(scores_object, T_obj2cam, relative_pose, verbose) cam_extrinsic, object_poses = _center_extrinsics( cam_extrinsic, object_poses) # ensure camera 0 is the world center point3d_coord, pid2d_to_pid3d = calc_3d_object_points( calib_object_points3d, object_poses, point2d_fid, point2d_cid, point2d_mid) return cam_extrinsic, point3d_coord, pid2d_to_pid3d, object_poses
def earliest_ancestor(ancestors, starting_node): """ 10 / 1 2 4 11 \ / / \ / 3 5 8 \ / \ \ 6 7 9 Write a function that, given the dataset and the ID of an individual in the dataset, returns their earliest known ancestor – the one at the farthest distance from the input individual. If there is more than one ancestor tied for "earliest", return the one with the lowest numeric ID. If the input individual has no parents, the function should return -1. """ # UPER # use bft # use len(longest) # if len(longest) == 1 then return -1 # check the length of the set of vertices for the longest # if there is no child return -1 # while there is an edge: # ancestor[0] is parent ancestor[1] is child # starting node has no parent then return -1 # initialize with the starting node is equal to the child # after that the child becomes the parent # graph = Graph() # # relatives is a node # for relatives in ancestors: # for relative in relatives: # graph.add_vertex(relative) # # print('GRAPHXXXXXX',graph.vertices) # for relatives in ancestors: # graph.add_edge(relatives[1],relatives[0]) # print('GRAPHXXXXXX',graph.vertices) graph = Graph() for pair in ancestors: graph.add_vertex(pair[0]) graph.add_vertex(pair[1]) graph.add_edge(pair[1], pair[0]) print('XXXXXVERTICESXXXX', graph.vertices) q = Queue() q.enqueue([starting_node]) max_path_len = 1 earliest_ancestor = -1 while q.size() > 0: path = q.dequeue() v = path[-1] # if path is longer or equal and value is smaller, or path is longer if (len(path) >= max_path_len and v < earliest_ancestor) or (len(path) > max_path_len): earliest_ancestor = v max_path_len = len(path) for neighbor in graph.vertices[v]: path_copy = list(path) path_copy.append(neighbor) q.enqueue(path_copy) return earliest_ancestor
# Fill this out with directions to walk # traversal_path = ['n', 'n'] traversal_path = [] room_graph = {} for i in range(len(origin_room_graph)): room_graph[i] = origin_room_graph[i] #Created Graph Object for DFS Method gg = Graph() for i in room_graph: gg.add_vertex(i) for i in room_graph: for key in room_graph[i][1]: gg.add_edge(i, room_graph[i][1][key]) # copy_of_origin_room_graph = origin_room_graph # for key in copy_of_origin_room_graph: # copy_of_origin_room_graph[key][1]) # print(origin_room_graph[1][1]) def get_neighbors(room): neighbors = [] if room.n_to is not None: neighbors.append(room.n_to.id) if room.s_to is not None: neighbors.append(room.s_to.id) if room.w_to is not None:
for i in range(len(chord_sets)): neighbor_near_list.update(neighbor_near(chord_sets[i])) for i in range(len(chord_sets)): neighbor_far_list.update(neighbor_far(chord_sets[i])) # Create distance table and save it to file. # Later, we can look up the table to find distance between 2 chords w1 = 1 w2 = 3 weight_table =[] count = 0 from datetime import datetime for chord in named_chords_encode: print str(datetime.now()) print count g = Graph() for i in range(1,4096): g.add_vertex(i) for i in range(1,4096): for j in neighbor_near_list[i]: g.add_edge(i,j,w1) for k in neighbor_far_list[i]: g.add_edge(i,k,w2) dijkstra(g,g.get_vertex(chord)) for i in range(1,4096): n = g.get_vertex(i).get_distance() weight_table.append([chord,i,n]) count = count + 1 pickle.dump(distance_table,open(config.distance_table, "wb" ) )