55
66class G2OVisualizer :
77 def __init__ (self , title ):
8+ """
9+ initializes a Figure and Axes object for the visualization of data using
10+ Matplotlib. It sets up the graphical interface, defines the title, sets
11+ limits for the axes, and flushes events to display the figure on the screen.
12+
13+ Args:
14+ title (str): title of the graphical window displayed by Matplotlib.
15+
16+ """
817 self .title = title
918
1019 # Configurar la ventana gráfica interactiva de Matplotlib
@@ -22,12 +31,70 @@ def __init__(self, title):
2231 # self.ax.set_ylim([-3000, 3000]) # Por ejemplo, límites para el eje y de 0 a 10
2332
2433 def edges_coord (self , edges , dim ):
34+ """
35+ takes an iterable of edges and a dimension, it returns a sequence of floats
36+ representing the coordinates of the vertices of each edge, followed by
37+ three `None` values to indicate the end of the sequence.
38+
39+ Args:
40+ edges (iterable/collection containing edges as its values.): 2-D graph
41+ edges, which are used to estimate their corresponding coordinate
42+ values for each dimension.
43+
44+ - `edges` is an instance of `List[Edge']`, where `Edge' =
45+ Tuple[Void, Void]`.
46+ - Each element in `edges` represents a pair of vertices connected
47+ by an edge. The first element in each pair is the start vertex,
48+ while the second element is the end vertex.
49+ - The `dim` parameter specifies the dimensionality of the
50+ coordinates for each vertex in the edges.
51+ dim (int): dimensionality of the space in which the coordinates are estimated.
52+
53+ """
2554 for e in edges :
2655 yield e .vertices ()[0 ].estimate ()[dim ]
2756 yield e .vertices ()[1 ].estimate ()[dim ]
2857 yield None
2958
3059 def update_graph (self , optimizer , ground_truth = None ):
60+ """
61+ updates the graph shown on a figure with the latest measurements from an
62+ optimizer, ground truth data (if provided), and poses of the vertices.
63+
64+ Args:
65+ optimizer (`g2o.Optimizer` object, as seen by inspecting it directly
66+ with Python's built-in `inspect` module, without further evaluation
67+ or contextualizing.): 3D object pose optimization instance, which
68+ provides the updated vertices and edges information through the
69+ `edges()` and `vertices()` methods.
70+
71+ 1/ `edges`: A list of edges in the graph, where each edge is an
72+ instance of `g2o.Edge`.
73+ 2/ `vertices`: A dictionary containing the positions of the
74+ vertices in the graph, where each key is a vertex index and each
75+ value is a 3D vector representing the vertex position.
76+ 3/ `edges_coord`: A function that maps an edge list to a 2D list
77+ of coordinate pairs, where each pair represents a point on the edge.
78+ 4/ `poses`: A list of 3D vectors representing the poses of the
79+ vertices in the graph.
80+ 5/ `measurments`: A list of 2D points representing the measurements
81+ obtained from the optimizer.
82+ 6/ `rotation`: An instance of `g2o.Quaternion` representing the
83+ rotation of each vertex in the graph.
84+ ground_truth (ndarray or a Python object representing a 2D numpy array
85+ containing the ground truth pose coordinates for the objects in
86+ the scene.): 3D positions of the robot's body or reference points,
87+ which are used to compare and visualize the estimated poses from
88+ the optimization process.
89+
90+ - If `ground_truth` is not `None`, it represents the ground truth
91+ data for the robot pose and measurement edges.
92+ - `ground_truth[0]` and `ground_truth[1]` denote the positions
93+ of the robot in SE(2) format, i.e., (x, y, z) where x, y, and z
94+ are the coordinates in the world frame.
95+ - `ground_truth.shape` gives the number of pose data points.
96+
97+ """
3198 self .ax .clear ()
3299 # Fijar límites de los ejes
33100 # self.ax.set_xlim([-3000, 3000]) # Por ejemplo, límites para el eje x de 0 a 10
0 commit comments