Implement SE(2) and SE(3) offset parameter classes

graphslam/g2o_parameters.py

@@ -7,8 +7,8 @@
 
 from abc import ABC, abstractmethod
 
-# from graphslam.pose.se2 import PoseSE2
-# from graphslam.pose.se3 import PoseSE3
+from graphslam.pose.se2 import PoseSE2
+from graphslam.pose.se3 import PoseSE3
 
 
 class BaseG2OParameter(ABC):
@@ -57,7 +57,109 @@ def from_g2o(cls, line):
 
         Returns
         -------
-        BaseParameter, None
+        BaseG2OParameter, None
             The instantiated parameter object, or ``None`` if ``line`` does not correspond to this parameter type
 
         """
+
+
+class G2OParameterSE2Offset(BaseG2OParameter):
+    """A class for storing a g2o :math:`SE(2)` offset parameter.
+
+    Attributes
+    ----------
+    key : tuple[str, int]
+        A tuple of the form ``("PARAMS_SE2OFFSET", id)``
+    value : graphslam.pose.se2.PoseSE2
+        The offset
+
+    """
+
+    def to_g2o(self):
+        """Export the :math:`SE(2)` offset parameter to the .g2o format.
+
+        Returns
+        -------
+        str
+            The parameter in .g2o format
+
+        """
+        return "PARAMS_SE2OFFSET {} {} {} {}\n".format(self.key[1], self.value[0], self.value[1], self.value[2])
+
+    @classmethod
+    def from_g2o(cls, line):
+        """Load an :math:`SE(2)` offset parameter from a line in a .g2o file.
+
+        Parameters
+        ----------
+        line : str
+            The line from the .g2o file
+
+        Returns
+        -------
+        G2OParameterSE2Offset, None
+            The instantiated parameter object, or ``None`` if ``line`` does not correspond to an :math:`SE(2)` offset parameter
+
+        """
+        if not line.startswith("PARAMS_SE2OFFSET "):
+            return None
+
+        numbers = line[len("PARAMS_SE2OFFSET "):].split()  # fmt: skip
+        arr = [float(number) for number in numbers[1:]]
+        return cls(("PARAMS_SE2OFFSET", int(numbers[0])), PoseSE2([arr[0], arr[1]], arr[2]))
+
+
+class G2OParameterSE3Offset(BaseG2OParameter):
+    """A class for storing a g2o :math:`SE(3)` offset parameter.
+
+    Attributes
+    ----------
+    key : tuple[str, int]
+        A tuple of the form ``("PARAMS_SE3OFFSET", id)``
+    value : graphslam.pose.se3.PoseSE3
+        The offset
+
+    """
+
+    def to_g2o(self):
+        """Export the :math:`SE(3)` offset parameter to the .g2o format.
+
+        Returns
+        -------
+        str
+            The parameter in .g2o format
+
+        """
+        return "PARAMS_SE3OFFSET {} {} {} {} {} {} {} {}\n".format(
+            self.key[1],
+            self.value[0],
+            self.value[1],
+            self.value[2],
+            self.value[3],
+            self.value[4],
+            self.value[5],
+            self.value[6],
+        )
+
+    @classmethod
+    def from_g2o(cls, line):
+        """Load an :math:`SE(3)` offset parameter from a line in a .g2o file.
+
+        Parameters
+        ----------
+        line : str
+            The line from the .g2o file
+
+        Returns
+        -------
+        G2OParameterSE3Offset, None
+            The instantiated parameter object, or ``None`` if ``line`` does not correspond to an :math:`SE(3)` offset parameter
+
+        """
+        if not line.startswith("PARAMS_SE3OFFSET "):
+            return None
+
+        # // PARAMS_SE3OFFSET id x y z qw qx qy qz
+        numbers = line[len("PARAMS_SE3OFFSET "):].split()  # fmt: skip
+        arr = [float(number) for number in numbers[1:]]
+        return cls(("PARAMS_SE3OFFSET", int(numbers[0])), PoseSE3(arr[:3], arr[3:]))

tests/test_g2o_parameters.py

@@ -1,14 +1,43 @@
 # Copyright (c) 2020 Jeff Irion and contributors
 
-"""Unit tests for the ``BaseEdge`` class.
+"""Unit tests for the g2o parameters classes.
 
 """
 
 
 import unittest
 
-# from graphslam.g2o_parameters import BaseG2OParameter
+from graphslam.g2o_parameters import G2OParameterSE2Offset, G2OParameterSE3Offset
+from graphslam.pose.se2 import PoseSE2
+from graphslam.pose.se3 import PoseSE3
 
 
-class TestBaseEdge(unittest.TestCase):
-    """Tests for the ``BaseG2OParameter`` class."""
+class TestG2OParameterSE2Offset(unittest.TestCase):
+    """Tests for the ``G2OParameterSE2Offset`` class."""
+
+    def test_to_g2o_from_g2o(self):
+        """Test the `to_g2o` and `from_g2o` methods."""
+        param = G2OParameterSE2Offset(("PARAMS_SE2OFFSET", 2), PoseSE2([1.0, 2.0], 3.0))
+
+        param2 = G2OParameterSE2Offset.from_g2o(param.to_g2o())
+
+        self.assertTupleEqual(param.key, param2.key)
+        self.assertTrue(param.value.equals(param2.value))
+
+        self.assertIsNone(G2OParameterSE2Offset.from_g2o("bologna"))
+
+
+class TestG2OParameterSE3Offset(unittest.TestCase):
+    """Tests for the ``G2OParameterSE3Offset`` class."""
+
+    def test_to_g2o_from_g2o(self):
+        """Test the `to_g2o` and `from_g2o` methods."""
+        param = G2OParameterSE3Offset(("PARAMS_SE3OFFSET", 4), PoseSE3([1.0, 2.0, 3.0], [0.1, 0.2, 0.3, 0.4]))
+        param.value.normalize()
+
+        param2 = G2OParameterSE3Offset.from_g2o(param.to_g2o())
+
+        self.assertTupleEqual(param.key, param2.key)
+        self.assertTrue(param.value.equals(param2.value))
+
+        self.assertIsNone(G2OParameterSE3Offset.from_g2o("bologna"))