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)
