From 94811c93b1eccd8c5240734f326c6d1b4ea05321 Mon Sep 17 00:00:00 2001
From: Peter Corke <peter.i.corke@gmail.com>
Date: Sun, 19 Jan 2025 22:32:39 +1000
Subject: [PATCH] Add methods to convert `SE3` to/from the rvec, tvec format
 used for poses within OpenCV

---
 spatialmath/pose3d.py | 35 +++++++++++++++++++++++++++++++++++
 tests/test_pose3d.py  | 11 ++++++++++-
 2 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
index b4301d93..c5b7dc27 100644
--- a/spatialmath/pose3d.py
+++ b/spatialmath/pose3d.py
@@ -1292,6 +1292,21 @@ def delta(self, X2: Optional[SE3] = None) -> R6:
         else:
             return smb.tr2delta(self.A, X2.A)
 
+    def rtvec(self) -> Tuple[R3, R3]:
+        """
+        Convert to OpenCV-style rotation and translation vectors
+
+        :return: rotation and translation vectors
+        :rtype: ndarray(3), ndarray(3)
+
+        Many OpenCV functions accept pose as two 3-vectors: a rotation vector using
+        exponential coordinates and a translation vector.  This method combines them
+        into an SE(3) instance.
+
+        :seealso: :meth:`rtvec`
+        """
+        return SO3(self).log(twist=True), self.t
+
     def Ad(self) -> R6x6:
         r"""
         Adjoint of SE(3)
@@ -1833,6 +1848,26 @@ def Exp(cls, S: Union[R6, R4x4], check: bool = True) -> SE3:
         else:
             return cls(smb.trexp(S), check=False)
 
+    @classmethod
+    def RTvec(cls, rvec: ArrayLike3, tvec: ArrayLike3) -> Self:
+        """
+        Construct a new SE(3) from OpenCV-style rotation and translation vectors
+
+        :param rvec: rotation as exponential coordinates
+        :type rvec: ArrayLike3
+        :param tvec: translation vector
+        :type tvec: ArrayLike3
+        :return: An SE(3) instance
+        :rtype: SE3 instance
+
+        Many OpenCV functions (such as pose estimation) return pose as two 3-vectors: a
+        rotation vector using exponential coordinates and a translation vector.  This
+        method combines them into an SE(3) instance.
+
+        :seealso: :meth:`rtvec`
+        """
+        return SE3.Rt(smb.trexp(rvec), tvec)
+
     @classmethod
     def Delta(cls, d: ArrayLike6) -> SE3:
         r"""
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
index d6a941c3..34f1941e 100755
--- a/tests/test_pose3d.py
+++ b/tests/test_pose3d.py
@@ -254,7 +254,6 @@ def test_conversion(self):
         nt.assert_array_almost_equal(q.SO3(), R_from_q)
         nt.assert_array_almost_equal(q.SO3().UnitQuaternion(), q)
 
-
     def test_shape(self):
         a = SO3()
         self.assertEqual(a._A.shape, a.shape)
@@ -1339,6 +1338,16 @@ def test_functions_vect(self):
         # .T
         pass
 
+    def test_rtvec(self):
+        # OpenCV compatibility functions
+        T = SE3.RTvec([0, 1, 0], [2, 3, 4])
+        nt.assert_equal(T.t, [2, 3, 4])
+        nt.assert_equal(T.R, SO3.Ry(1))
+
+        rvec, tvec = T.rtvec()
+        nt.assert_equal(rvec, [0, 1, 0])
+        nt.assert_equal(tvec, [2, 3, 4])
+
 
 # ---------------------------------------------------------------------------------------#
 if __name__ == "__main__":