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