def main():
"""Plot the unoptimized and optimized graphs.
"""
GRAPH.generate_unoptimized_graph()
GRAPH.optimize_graph()
GRAPH.generate_maximization_params()
GRAPH.tune_weights()
print("Weights: ", GRAPH.weights)
print(np.sqrt(np.exp(GRAPH.weights)) * 3)
unoptimized_trajectory = optimizer_to_map(GRAPH.vertices,
GRAPH.unoptimized_graph)
optimized_trajectory = optimizer_to_map(GRAPH.vertices,
GRAPH.optimized_graph)
print('Optimized chi2: ', GRAPH.optimized_graph.chi2())
weights = np.reshape([], (0, 18))
for _ in range(0):
GRAPH.expectation_maximization_once()
print(GRAPH.maximization_success)
print(GRAPH.g2o_status)
print(GRAPH.maximization_results.message)
weights = np.vstack([weights, GRAPH.weights])
print(GRAPH.weights)
fig = plt.figure()
axes = fig.add_subplot(111, projection='3d')
tag_marker = '^'
waypoint_marker = 's'
location_marker = '.'
axes.plot(unoptimized_trajectory['locations'][:, 0],
unoptimized_trajectory['locations'][:, 1],
unoptimized_trajectory['locations'][:, 2],
location_marker,
label='Uncorrected Path')
axes.plot(optimized_trajectory['locations'][:, 0],
optimized_trajectory['locations'][:, 1],
optimized_trajectory['locations'][:, 2],
location_marker,
label='Corrected Path')
axes.scatter(unoptimized_trajectory['tags'][:, 0],
unoptimized_trajectory['tags'][:, 1],
unoptimized_trajectory['tags'][:, 2],
marker=tag_marker,
s=100,
label='Uncorrected Tags')
axes.scatter(optimized_trajectory['tags'][:, 0],
optimized_trajectory['tags'][:, 1],
optimized_trajectory['tags'][:, 2],
marker=tag_marker,
s=100,
label='Corrected Tags')
axes.plot(unoptimized_trajectory['waypoints'][:, 0],
unoptimized_trajectory['waypoints'][:, 1],
unoptimized_trajectory['waypoints'][:, 2],
waypoint_marker,
label='Uncorrected Waypoints')
axes.plot(optimized_trajectory['waypoints'][:, 0],
optimized_trajectory['waypoints'][:, 1],
optimized_trajectory['waypoints'][:, 2],
waypoint_marker,
label='Corrected Waypoints')
axes.legend()
plt.show()
def optimize_map(x, tune_weights=False, visualize=False):
test_graph = convert_json.as_graph(x)
# higher means more noisy (note: the uncertainty estimates of translation seem to be pretty over optimistic, hence the large correction here)
sensible_default_weights = np.array([
-6., -6., -6., -6., -6., -6., 18, 18, 18, 18, 18, 18, 0., 0., 0., -1,
-1, 1e2
])
trust_odom = np.array([
-3., -3., -3., -3., -3., -3., 10.6, 10.6, 10.6, 10.6, 10.6, 10.6, 0.,
0., 0., -1, -1, 1e2
])
trust_tags = np.array([
10, 10, 10, 10, 10, 10, -10.6, -10.6, -10.6, -10.6, -10.6, -10.6, 0, 0,
0, -1e2, 3, 3
])
test_graph.weights = sensible_default_weights
# Load these weights into the graph
test_graph.update_edges()
test_graph.generate_unoptimized_graph()
all_tags_original = test_graph.get_tags_all_position_estimate()
starting_map = graph_utils.optimizer_to_map(test_graph.vertices,
test_graph.unoptimized_graph)
original_tag_verts = starting_map['tags']
if tune_weights:
test_graph.expectation_maximization_once()
print("tuned weights", test_graph.weights)
# Create the g2o object and optimize
test_graph.generate_unoptimized_graph()
test_graph.optimize_graph()
# Change vertex estimates based off the optimized graph
test_graph.update_vertices()
resulting_map = graph_utils.optimizer_to_map(test_graph.vertices,
test_graph.optimized_graph)
locations = resulting_map['locations']
tag_verts = resulting_map['tags']
waypoint_verts = resulting_map['waypoints']
if visualize:
all_tags = test_graph.get_tags_all_position_estimate()
f = plt.figure()
ax = f.add_subplot(111, projection='3d')
plt.plot(locations[:, 0],
locations[:, 1],
locations[:, 2],
'.',
c='b',
label='Odom Vertices')
plt.plot(original_tag_verts[:, 0],
original_tag_verts[:, 1],
original_tag_verts[:, 2],
'o',
c='c',
label='Tag Vertices Original')
plt.plot(tag_verts[:, 0],
tag_verts[:, 1],
tag_verts[:, 2],
'o',
c='r',
label='Tag Vertices')
for vert in tag_verts:
ax.text(vert[0],
vert[1],
vert[2],
str(int(vert[-1])),
color='black')
for tag_vert in tag_verts:
R = Quaternion(tag_vert[3:-1]).rotation_matrix()
axis_to_color = ['r', 'g', 'b']
for axis_id in range(3):
ax.quiver(tag_vert[0],
tag_vert[1],
tag_vert[2],
R[0, axis_id],
R[1, axis_id],
R[2, axis_id],
length=1,
color=axis_to_color[axis_id])
plt.plot(waypoint_verts[1][:, 0],
waypoint_verts[1][:, 1],
waypoint_verts[1][:, 2],
'o',
c='y',
label='Waypoint Vertices')
for vert_idx in range(len(waypoint_verts[0])):
vert = waypoint_verts[1][vert_idx]
waypoint_name = waypoint_verts[0][vert_idx]['name']
ax.text(vert[0], vert[1], vert[2], waypoint_name, color='black')
plt.plot(all_tags[:, 0],
all_tags[:, 1],
all_tags[:, 2],
'.',
c='g',
label='All Tag Edges')
plt.plot(all_tags_original[:, 0],
all_tags_original[:, 1],
all_tags_original[:, 2],
'.',
c='m',
label='All Tag Edges Original')
tag_edge_std_dev_before_and_after = compare_std_dev(
all_tags, all_tags_original)
tag_vertex_shift = original_tag_verts - tag_verts
print("tag_vertex_shift", tag_vertex_shift)
axis_equal(ax)
plt.legend()
plt.show()
return tag_verts, waypoint_verts
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., -1e1, -1e1, 1e1
])
# Load these weights into the graph
test_graph.update_edges()
# Create the g2o object and optimize
test_graph.generate_unoptimized_graph()
test_graph.optimize_graph()
# Change vertex estimates based off the optimized graph
test_graph.update_vertices()
resulting_map = graph_utils.optimizer_to_map(
test_graph.vertices, test_graph.optimized_graph)
locations = resulting_map['locations']
tag_verts = resulting_map['tags']
# tag_verts =
# tags = resulting_map['tags']
edges = test_graph.ordered_odometry_edges()[0]
path = test_graph.integrate_path(edges, [
2.38298111e+01, 6.18518412e-01, - 2.23812237e+01,
-1.15648886e-02, 1.37184479e-01, 7.07669616e-01, -6.93001000e-01
])
tags = test_graph.get_tags_all_position_estimate()
f = plt.figure()
f.add_subplot(111, projection='3d')
import pickle
import itertools
import numpy as np
from graph_utils import optimizer_to_map
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial.transform import Rotation as R
with open('converted-data/academic_center.pkl', 'rb') as data:
graph = pickle.load(data)
graph.generate_unoptimized_graph()
unoptimized_map = optimizer_to_map(graph.vertices, graph.unoptimized_graph)
graph.optimize_graph()
optimized_map = optimizer_to_map(graph.vertices, graph.optimized_graph)
def compute_transformation(start, end):
"""
compute the transformation from start position to end
:param start: start position as a pose np array of [x, y, z, qx, qy, qz, 1]
:param end: end position as a pose np array of [x, y, z, qx, qy, qz, 1]
:return: numpy array of
"""
translation = list(end[:3] - start[:3])
start_rotation = R.from_quat(start[3:])
end_rotation = R.from_quat(end[3:])
rotation = end_rotation.as_dcm() * np.linalg.inv(start_rotation.as_dcm())
rotation = list(R.from_dcm(rotation).as_quat())
return np.array(translation + rotation)
from graph_utils import optimizer_to_map
import pickle
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
data = "converted-data/test_work.pkl"
with open(data, 'rb') as data:
graph = pickle.load(data)
graph = graph.get_subgraph(1000, 1500)
graph.generate_unoptimized_graph()
optimizer = graph.unoptimized_graph
graph_map = optimizer_to_map(graph.vertices, optimizer)
tags_estimate = graph.get_tags_all_position_estimate()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
tagMarker = '^'
locationMarker = '.'
ax.scatter(graph_map['tags'][:, 0],
graph_map['tags'][:, 1],
graph_map['tags'][:, 2],
marker=tagMarker,
s=100,
label='Tags')
ax.plot(graph_map['locations'][:, 0],
graph_map['locations'][:, 1],