Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Jan 26, 2025
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions spatialmath/pose3d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this for https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac ?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

More for something like pose estimation or camera projection.

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)
Expand Down Expand Up @@ -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"""
Expand Down
11 changes: 10 additions & 1 deletion tests/test_pose3d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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__":
Expand Down
Loading