Add methods to convert SE3 to/from the rvec, tvec format used by OpenCV (bdaiinstitute#157)

petercorke · web-flow · commit 8c6d42262941 · 2025-01-26T01:00:26.000-05:00
diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
@@ -1315,6 +1315,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)
@@ -1856,6 +1871,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
@@ -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)
@@ -1370,6 +1369,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__":
