# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3
#     language: python
#     name: python3
# ---

# +
# %matplotlib notebook
import json
import as_graph
import numpy as np
import graph_utils
import matplotlib.pyplot as plt

# Testing code
with open('data/round1.json', 'r') as f:
    x = json.load(f)

test_graph = as_graph.as_graph(x, prescaling_opt=as_graph.PrescalingOptEnum.FULL_COV)
test_graph.weights = np.array([
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 1e2, 1e2
])
test_graph.update_edge_information()

# Create the g2o object and optimize
test_graph.generate_unoptimized_graph()
original_tags = test_graph.get_tags_all_position_estimate()
original_map = graph_utils.optimizer_to_map(
        test_graph.vertices, test_graph.unoptimized_graph)
test_graph.optimize_graph()

# Change vertex estimates based off the optimized graph
test_graph.update_vertices_estimates()

resulting_map = graph_utils.optimizer_to_map(
    test_graph.vertices, test_graph.optimized_graph)

original_locations = original_map['locations']
original_tag_verts = original_map['tags']

locations = resulting_map['locations']
tag_verts = resulting_map['tags']

# + pycharm={"name": "#%%\n"}
edges = test_graph.get_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')
plt.plot(locations[:, 0], locations[:, 1], locations[:, 2], '.', c='b', label='Odom Vertices')
plt.plot(tag_verts[:, 0], tag_verts[:, 1], tag_verts[:, 2], 'o', c='r', label='Tag Vertices')
plt.plot(tags[:, 0], tags[:, 1], tags[:, 2], '.', c='g', label='All Tag Edges')
plt.plot(original_locations[:, 0], original_locations[:, 1], original_locations[:, 2], '.', c='k', label='Original Odom Vertices')
plt.plot(original_tag_verts[:, 0], original_tag_verts[:, 1], original_tag_verts[:, 2], 'o', c='m', label='Original Tag Vertices')
plt.plot(original_tags[:, 0], original_tags[:, 1], original_tags[:, 2], '.', c='c', label='All Tag Edges')
plt.legend()
plt.savefig('optimized.png')
plt.show()

StarcoderdataJupyterScriptsDedupFiltered