Skip to content

Commit

Permalink
Merge 4796f07 into 06f2c56
Browse files Browse the repository at this point in the history
  • Loading branch information
JeffLIrion committed Nov 14, 2023
2 parents 06f2c56 + 4796f07 commit e66446f
Show file tree
Hide file tree
Showing 4 changed files with 92 additions and 1 deletion.
45 changes: 44 additions & 1 deletion graphslam/graph.py
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@

from .edge.base_edge import BaseEdge
from .edge.edge_odometry import EdgeOdometry
from .g2o_parameters import G2OParameterSE2Offset, G2OParameterSE3Offset
from .vertex import Vertex


Expand Down Expand Up @@ -300,6 +301,8 @@ class Graph(object):
A list of the edges (i.e., constraints) in the graph
_fixed_gradient_indices : set[int]
The set of gradient indices (i.e., `Vertex.gradient_index`) for vertices that are fixed
_g2o_params : dict, None
A dictionary where the values are `BaseG2OParameter` objects
_gradient : numpy.ndarray, None
The gradient :math:`\mathbf{b}` of the :math:`\chi^2` error, or ``None`` if it has not yet been computed
_hessian : scipy.sparse.lil_matrix, None
Expand All @@ -324,6 +327,8 @@ def __init__(self, edges, vertices):

self._len_gradient = None

self._g2o_params = None

self._initialize()

def _initialize(self):
Expand Down Expand Up @@ -511,6 +516,10 @@ def to_g2o(self, outfile):
"""
with open(outfile, "w") as f:
if self._g2o_params:
for g2o_param in self._g2o_params.values():
f.write(g2o_param.to_g2o())

for v in self._vertices:
f.write(v.to_g2o())

Expand Down Expand Up @@ -538,11 +547,37 @@ def load_g2o(cls, infile, custom_edge_types=None):
"""
edges = []
vertices = []
g2o_params = {}

custom_edge_types = custom_edge_types or []
for edge_type in custom_edge_types:
assert issubclass(edge_type, BaseEdge)

param_types = [G2OParameterSE2Offset, G2OParameterSE3Offset]

def param_from_g2o(line, param_types):
"""Load a parameter from a .g2o line.
Parameters
----------
line : str
A line from a .g2o file
param_types : list[type]
A list of parameter types, which must be subclasses of ``BaseG2OParameter``
Returns
-------
BaseG2OParameter, None
The instantiated parameter object, or ``None`` if the line does not correspond to any of the parameter types
"""
for param_type in param_types:
param_or_none = param_type.from_g2o(line)
if param_or_none:
return param_or_none

return None

def custom_edge_from_g2o(line, custom_edge_types):
"""Load a custom edge from a .g2o line.
Expand Down Expand Up @@ -587,9 +622,17 @@ def custom_edge_from_g2o(line, custom_edge_types):
edges.append(edge_or_none)
continue

# Parameters
param_or_none = param_from_g2o(line, param_types)
if param_or_none:
g2o_params[param_or_none.key] = param_or_none
continue

_LOGGER.warning("Line not supported -- '%s'", line.rstrip())

return cls(edges, vertices)
ret = cls(edges, vertices)
ret._g2o_params = g2o_params
return ret

def plot(self, vertex_color="r", vertex_marker="o", vertex_markersize=3, edge_color="b", title=None):
"""Plot the graph.
Expand Down
46 changes: 46 additions & 0 deletions tests/test_load.py
Original file line number Diff line number Diff line change
Expand Up @@ -164,3 +164,49 @@ def test_load_custom_edge_with_to_g2o_with_from_g2o(self):
with mock.patch("graphslam.graph.open", open_fake_file):
g2 = Graph.load_g2o("test.g2o", [EdgeWithToG2OWithFromG2O])
self.assertTrue(g.equals(g2))

# pylint: disable=protected-access
def test_to_g2o_from_g2o_with_parameters_se2(self):
"""Test that writing and loading parameters works."""
infile = os.path.join(os.path.dirname(__file__), "test_se2.g2o")

g = Graph.load_g2o(infile)

with mock.patch("graphslam.graph.open", open_fake_file):
g.to_g2o("test.g2o")

with mock.patch("graphslam.graph.open", open_fake_file):
g2 = Graph.load_g2o("test.g2o")

for g_ in [g, g2]:
self.assertIsNotNone(g_._g2o_params)
self.assertEqual(len(g_._g2o_params), 1)
self.assertTrue(("PARAMS_SE2OFFSET", 0) in g_._g2o_params)

p1 = g._g2o_params[("PARAMS_SE2OFFSET", 0)].value
p2 = g2._g2o_params[("PARAMS_SE2OFFSET", 0)].value

self.assertTrue(p1.equals(p2))

# pylint: disable=protected-access
def test_to_g2o_from_g2o_with_parameters_se3(self):
"""Test that writing and loading parameters works."""
infile = os.path.join(os.path.dirname(__file__), "test_se3.g2o")

g = Graph.load_g2o(infile)

with mock.patch("graphslam.graph.open", open_fake_file):
g.to_g2o("test.g2o")

with mock.patch("graphslam.graph.open", open_fake_file):
g2 = Graph.load_g2o("test.g2o")

for g_ in [g, g2]:
self.assertIsNotNone(g_._g2o_params)
self.assertEqual(len(g_._g2o_params), 1)
self.assertTrue(("PARAMS_SE3OFFSET", 0) in g_._g2o_params)

p1 = g._g2o_params[("PARAMS_SE3OFFSET", 0)].value
p2 = g2._g2o_params[("PARAMS_SE3OFFSET", 0)].value

self.assertTrue(p1.equals(p2))
1 change: 1 addition & 0 deletions tests/test_se2.g2o
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
PARAMS_SE2OFFSET 0 1.0 2.0 0.123
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.000000 0.000000 1.000000
VERTEX_XY 2 0.000000 0.000000
Expand Down
1 change: 1 addition & 0 deletions tests/test_se3.g2o
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
PARAMS_SE3OFFSET 0 1.0 2.0 3.0 0.5 0.5 0.5 0.5
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 0.000000 1.000000 2.000000 0.500000 0.500000 0.500000 0.500000
VERTEX_XY 2 0.000000 0.000000
Expand Down

0 comments on commit e66446f

Please sign in to comment.