Add identity() method for poses (#46)

* Add identity() method for poses

* Fix

graphslam/pose/base_pose.py

@@ -10,6 +10,18 @@
 class BasePose(np.ndarray):
     """A base class for poses."""
 
+    @classmethod
+    def identity(cls):
+        """Return the identity pose.
+
+        Returns
+        -------
+        BasePose
+            The identity pose
+
+        """
+        raise NotImplementedError
+
     # pylint: disable=arguments-differ
     def copy(self):
         """Return a copy of the pose.

graphslam/pose/r2.py

@@ -26,6 +26,18 @@ def __new__(cls, position):
         obj = np.asarray(position, dtype=np.float64).view(cls)
         return obj
 
+    @classmethod
+    def identity(cls):
+        """Return the identity pose.
+
+        Returns
+        -------
+        PoseR2
+            The identity pose
+
+        """
+        return PoseR2([0.0, 0.0])
+
     def copy(self):
         """Return a copy of the pose.
 

graphslam/pose/r3.py

@@ -26,6 +26,18 @@ def __new__(cls, position):
         obj = np.asarray(position, dtype=np.float64).view(cls)
         return obj
 
+    @classmethod
+    def identity(cls):
+        """Return the identity pose.
+
+        Returns
+        -------
+        PoseR3
+            The identity pose
+
+        """
+        return PoseR3([0.0, 0.0, 0.0])
+
     def copy(self):
         """Return a copy of the pose.
 

graphslam/pose/se2.py

@@ -33,6 +33,18 @@ def __new__(cls, position, orientation):
         obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls)
         return obj
 
+    @classmethod
+    def identity(cls):
+        """Return the identity pose.
+
+        Returns
+        -------
+        PoseSE2
+            The identity pose
+
+        """
+        return PoseSE2([0.0, 0.0], 0.0)
+
     def copy(self):
         """Return a copy of the pose.
 

graphslam/pose/se3.py

@@ -31,6 +31,18 @@ def __new__(cls, position, orientation):
         # fmt: on
         return obj
 
+    @classmethod
+    def identity(cls):
+        """Return the identity pose.
+
+        Returns
+        -------
+        PoseSE3
+            The identity pose
+
+        """
+        return PoseSE3([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
+
     def normalize(self):
         """Normalize the quaternion portion of the pose."""
         sgn = 1.0 if self[6] >= 0.0 else -1.0

tests/test_base_pose.py

@@ -13,6 +13,11 @@
 class TestBasePose(unittest.TestCase):
     """Tests for the ``PoseR2`` class."""
 
+    def test_identity(self):
+        """Test that the ``identity`` method is not implemented."""
+        with self.assertRaises(NotImplementedError):
+            BasePose.identity()
+
     def test_to_array(self):
         """Test that the ``to_array`` method is not implemented."""
         p = BasePose([])

tests/test_pose_r2.py

@@ -96,6 +96,9 @@ def test_add(self):
         r2a += r2b
         self.assertAlmostEqual(np.linalg.norm((r2a - expected).to_array()), 0.0)
 
+        expected2 = expected + PoseR2.identity()
+        self.assertAlmostEqual(np.linalg.norm((expected2 - expected).to_array()), 0.0)
+
     def test_sub(self):
         """Test that the overloaded ``__sub__`` method works as expected."""
         r2a = PoseR2([1, 2])
@@ -104,6 +107,9 @@ def test_sub(self):
         expected = PoseR2([2, 2])
         self.assertAlmostEqual(np.linalg.norm(((r2b - r2a) - expected).to_array()), 0.0)
 
+        expected2 = expected - PoseR2.identity()
+        self.assertAlmostEqual(np.linalg.norm((expected2 - expected).to_array()), 0.0)
+
     # ======================================================================= #
     #                                                                         #
     #                                Jacobians                                #

tests/test_pose_r3.py

@@ -93,6 +93,9 @@ def test_add(self):
         expected = PoseR3([4, 6, 8])
         self.assertAlmostEqual(np.linalg.norm(((r3a + r3b) - expected).to_array()), 0.0)
 
+        expected2 = expected + PoseR3.identity()
+        self.assertAlmostEqual(np.linalg.norm((expected2 - expected).to_array()), 0.0)
+
     def test_sub(self):
         """Test that the overloaded ``__sub__`` method works as expected."""
         r3a = PoseR3([1, 2, 3])
@@ -101,6 +104,9 @@ def test_sub(self):
         expected = PoseR3([2, 2, 2])
         self.assertAlmostEqual(np.linalg.norm(((r3b - r3a) - expected).to_array()), 0.0)
 
+        expected2 = expected - PoseR3.identity()
+        self.assertAlmostEqual(np.linalg.norm((expected2 - expected).to_array()), 0.0)
+
     # ======================================================================= #
     #                                                                         #
     #                                Jacobians                                #

tests/test_pose_se2.py

@@ -110,6 +110,9 @@ def test_add(self):
             expected = PoseSE2.from_matrix(np.dot(p1.to_matrix(), p2.to_matrix()))
             self.assertAlmostEqual(np.linalg.norm((p1 + p2).to_array() - expected.to_array()), 0.0)
 
+            expected2 = expected + PoseSE2.identity()
+            self.assertAlmostEqual(np.linalg.norm((expected2 - expected).to_array()), 0.0)
+
         # PoseSE2 (+) PoseR2
         for _ in range(10):
             p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
@@ -133,6 +136,10 @@ def test_sub(self):
             expected = np.dot(np.linalg.inv(p2.to_matrix()), p1.to_matrix())
             self.assertAlmostEqual(np.linalg.norm((p1 - p2).to_matrix() - expected), 0.0)
 
+            expected2 = PoseSE2.from_matrix(expected) - PoseSE2.identity()
+            expected2 = p1 - p2 - PoseSE2.identity()
+            self.assertAlmostEqual(np.linalg.norm(expected2.to_matrix() - expected), 0.0)
+
     # ======================================================================= #
     #                                                                         #
     #                                Jacobians                                #

tests/test_pose_se3.py

@@ -115,6 +115,9 @@ def test_add(self):
             expected = np.dot(p1.to_matrix(), p2.to_matrix())
             self.assertAlmostEqual(np.linalg.norm((p1 + p2).to_matrix() - expected), 0.0)
 
+            expected2 = (p1 + p2 + PoseSE3.identity()).to_matrix()
+            self.assertAlmostEqual(np.linalg.norm(expected2 - expected), 0.0)
+
         # PoseSE3 [+] numpy.ndarray
         for _ in range(10):
             p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
@@ -163,6 +166,9 @@ def test_sub(self):
             expected = np.dot(np.linalg.inv(p2.to_matrix()), p1.to_matrix())
             self.assertAlmostEqual(np.linalg.norm((p1 - p2).to_matrix() - expected), 0.0)
 
+            expected2 = (p1 - p2 - PoseSE3.identity()).to_matrix()
+            self.assertAlmostEqual(np.linalg.norm(expected2 - expected), 0.0)
+
     # ======================================================================= #
     #                                                                         #
     #                                Jacobians                                #