def run(location_geotable):
graph = Graph()
for index, row in location_geotable.iterrows():
graph.add_node(index, **{
'lat': row['Latitude'],
'lon': row['Longitude']
})
for node1_id in range(min(graph), max(graph)):
for node2_id in range(node1_id + 1, max(graph) + 1):
node1_d = graph.node[node1_id]
node2_d = graph.node[node2_id]
node1_ll = node1_d['lat'], node1_d['lon']
node2_ll = node2_d['lat'], node2_d['lon']
distance = get_distance(node1_ll, node2_ll).m
graph.add_edge(node1_id, node2_id, weight=distance)
tree = minimum_spanning_tree(graph)
total_distance = sum(
edge_d['weight']
for node1_id, node2_id, edge_d in tree.edges(data=True))
location_count = len(graph)
average_distance = divide_safely(total_distance, location_count, 0)
return [
('total_distance_between_locations_in_meters', total_distance),
('location_count', location_count),
('average_distance_between_locations_in_meters', average_distance),
]
def run(location_geotable):
graph = Graph()
for index, row in location_geotable.iterrows():
graph.add_node(index, {
'lat': row['Latitude'], 'lon': row['Longitude']})
for node1_id in xrange(min(graph), max(graph)):
for node2_id in xrange(node1_id + 1, max(graph) + 1):
node1_d = graph.node[node1_id]
node2_d = graph.node[node2_id]
node1_ll = node1_d['lat'], node1_d['lon']
node2_ll = node2_d['lat'], node2_d['lon']
distance = get_distance(node1_ll, node2_ll).m
graph.add_edge(node1_id, node2_id, weight=distance)
tree = minimum_spanning_tree(graph)
total_distance = sum(edge_d[
'weight'] for node1_id, node2_id, edge_d in tree.edges(data=True))
location_count = len(graph)
average_distance = divide_safely(total_distance, location_count, 0)
return [
('total_distance_between_locations_in_meters', total_distance),
('location_count', location_count),
('average_distance_between_locations_in_meters', average_distance),
]
def getMST(self):
''' Acquire the minimum spanning tree for the graph. '''
return alg.minimum_spanning_tree(self.graph)
def optimize_transforms (G):
"""
Optimize for transforms in G. Assumes G is_ready.
Returns a clique with relative transforms between all objects.
"""
if G.number_of_edges == 0:
return G.to_directed()
# Index maps from nodes and edges in the optimizer variable X.
# Also calculate the reverse map if needed.
node_map, edge_map = {}, {}
rev_map = {}
idx = 0
for obj in G.nodes_iter():
node_map[obj] = idx
rev_map[idx] = obj
idx += 1
for i,j in G.edges_iter():
if i < j:
edge_map[i,j] = idx
rev_map[idx] = i,j
else:
edge_map[j,i] = idx
rev_map[idx] = j,i
idx += 1
# Some predefined variables
I3 = np.eye(3)
def get_mat_from_x(X, i, j):
"""
X is a vertical stack of variables for transformation matrices (12 variables each).
Returns 3x4 matrix Ti or Tij by looking into the index maps.
"""
if j is None:
offset = node_map[i]*12
else:
if i < j:
offset = edge_map[i,j]*12
else:
offset = edge_map[j,i]*12
Xij = X[offset:offset+12]
return Xij.reshape([3,4], order='F')
def f_objective (X):
"""
Objective function to make transforms close to average transforms.
Sum of the norms of matrix differences between each Tij and
"""
obj = 0
for i,j in edge_map:
obj += nlg.norm(get_mat_from_x(X, i, j) - G[i][j]['avg_tfm'][0:3,:])
return obj
def f_constraints (X):
"""
Constraint function to force matrices to be valid transforms (R.T*R = I_3).
It also forces Tj = Ti*Tij.
Also, T0 = identity transform.
"""
con = []
Tis, Tijs = {},{}
for node in node_map:
Tis[node] = get_mat_from_x(X, node, None)
for i,j in edge_map:
Tijs[i,j] = get_mat_from_x(X, i, j)
# T0 --> identity transform.
con.append(nlg.norm(Tis[node_map.keys()[0]] - np.eye(4)[0:3,:]))
# Constrain Ris to be valid rotations
for node in node_map.keys()[1:]:
Ri = Tis[node][0:3,0:3]
con.append(nlg.norm(Ri.T.dot(Ri) - I3))
# Tj = Ti*Tij
# Rij.T*Rij = I
for i,j in edge_map:
Ti = np.r_[Tis[i],np.array([[0,0,0,1]])]
Tj = np.r_[Tis[j],np.array([[0,0,0,1]])]
Tij = np.r_[Tijs[i,j],np.array([[0,0,0,1]])]
Rij = Tij[0:3,0:3]
con.append(nlg.norm(Tj - Ti.dot(Tij)))
con.append(nlg.norm(Rij.T.dot(Rij) - I3))
return np.asarray(con)
### Setting initial values by walking through maximal spanning tree.
G_init = G.copy()
for i,j in G_init.edges_iter():
G_init[i][j]['weight'] = -1*G_init[i][j]['n']
G_init_tree = nxa.minimum_spanning_tree(G_init)
x_init = np.zeros(12*(G.number_of_nodes() + G.number_of_edges()))
node0 = G_init_tree.nodes()[0]
offset0 = node_map[node0]
x_init[offset0:offset0+9] = I3.reshape(9)
fringe = [node0]
seen = []
while len(fringe)>0:
node = fringe.pop(0)
if node in seen: continue
seen.append(node)
for n_node in G_init_tree.neighbors(node):
if n_node in seen: continue
fringe.append(n_node)
offset = node_map[n_node]*12
tfm = G.edge[node][n_node]['avg_tfm']
if node > n_node:
tfm = nlg.inv(tfm)
x_init[offset:offset+12] = tfm[0:3,:].reshape(12, order='F')
for i,j in edge_map:
offset = edge_map[i,j]*12
x_init[offset:offset+12] = G.edge[i][j]['avg_tfm'][0:3,:].reshape(12,order='F')
### ###
print "Initial x: ", x_init
(X, fx, _, _, _) = sco.fmin_slsqp(func=f_objective, x0=x_init, f_eqcons=f_constraints, iter=100, full_output=1)
# Create output optimal graph and copy edge transforms
G_opt = G.to_directed()
for i,j in edge_map:
Tij = np.r_[get_mat_from_x(X, i, j),np.array([[0,0,0,1]])]
Tji = nlg.inv(Tij)
G_opt.edge[i][j] = {'tfm':Tij}
G_opt.edge[j][i] = {'tfm':Tji}
# Add all edges to make clique
# Follow shortest path to get transform for edge
for i,j in itertools.combinations(sorted(G_opt.nodes()), 2):
if not G_opt.has_edge(i,j):
ij_path = nxa.shortest_path(G_opt, i, j)
Tij = G_opt.edge[ij_path[0]][ij_path[1]]['tfm']
for p in xrange(2,len(ij_path)):
Tij = Tij.dot(G_opt.edge[ij_path[p-1]][ij_path[p]]['tfm'])
Tji = nlg.inv(Tij)
G_opt.add_edge(i,j)
G_opt.add_edge(j,i)
G_opt.edge[i][j] = {'tfm':Tij}
G_opt.edge[j][i] = {'tfm':Tji}
for i in G_opt.nodes_iter():
G_opt.add_edge(i,i)
G_opt[i][i]['tfm'] = np.eye(4)
n = G_opt.number_of_nodes()
try:
assert G_opt.number_of_edges() == n**2
except AssertionError:
print "Not all edges found...? Fix this"
return G_opt
def getMST(self):
''' Acquire the minimum spanning tree for the graph. '''
return alg.minimum_spanning_tree(self.graph)
