Draw pose graph

[keevindoherty]

Would be cool to add functionality similar to the SE-Sync visualizer to show pose graphs.

Not sure what a good "standard input" for this would be. Minimally, we need a std::vector<VizPose> and an edge list std::vector<std::pair<size_t>> consisting of all edges (i,j) in the graph (plus maybe some bonus info about how to display the edge, e.g. color).

One possibility:

struct VizEdge {
size_t i;  // Edge departs node "i"
size_t j;  // Edge is incident to node "j"
Color3f color;  // Color override for edge i,j
}

void DrawGraph(const std::vector<VizPose> &trajectory, const std::vector<VizEdge> &edge_list);