()
| 51 | pass |
| 52 | |
| 53 | def make_plotter(): |
| 54 | import numpy as np |
| 55 | from mpl_toolkits.mplot3d import Axes3D |
| 56 | |
| 57 | fig = plt.figure() |
| 58 | ax = Axes3D(fig) |
| 59 | fig.add_axes(ax) |
| 60 | |
| 61 | ax_bounds = (-0.5, 0.5) # meters |
| 62 | ax.set(xlim=ax_bounds, ylim=ax_bounds, zlim=ax_bounds) |
| 63 | ax.view_init(azim=-140) # initial plot orientation |
| 64 | |
| 65 | vio_plot = ax.plot( |
| 66 | xs=[], ys=[], zs=[], |
| 67 | linestyle="-", |
| 68 | marker="" |
| 69 | ) |
| 70 | ax.set_xlabel("x (m)") |
| 71 | ax.set_ylabel("y (m)") |
| 72 | ax.set_zlabel("z (m)") |
| 73 | |
| 74 | title = ax.set_title("VIO trajectory") |
| 75 | |
| 76 | data = { c: [] for c in 'xyz' } |
| 77 | |
| 78 | control = { 'close': False } |
| 79 | fig.canvas.mpl_connect('close_event', lambda _: control.update({'close': True})) |
| 80 | |
| 81 | def update_data(vio_out): |
| 82 | if control['close']: return False |
| 83 | # supports two slightly different JSONL formats |
| 84 | if 'pose' in vio_out: vio_out = vio_out['pose'] |
| 85 | # SDK < 0.12 does not expose the TRACKING status |
| 86 | is_tracking = vio_out.get('status', 'TRACKING') == 'TRACKING' |
| 87 | for c in 'xyz': |
| 88 | val = vio_out['position'][c] |
| 89 | if not is_tracking: val = np.nan |
| 90 | data[c].append(val) |
| 91 | return True |
| 92 | |
| 93 | def update_graph(frames): |
| 94 | x, y, z = [np.array(data[c]) for c in 'xyz'] |
| 95 | vio_plot[0].set_data(x, y) |
| 96 | vio_plot[0].set_3d_properties(z) |
| 97 | return (vio_plot[0],) |
| 98 | |
| 99 | from matplotlib.animation import FuncAnimation |
| 100 | anim = FuncAnimation(fig, update_graph, interval=15, blit=True) |
| 101 | return update_data, anim |
| 102 | |
| 103 | if __name__ == '__main__': |
| 104 | plotter, anim = make_plotter() |
no test coverage detected