Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

Appearance settings

Commit 659ba24

Browse filesBrowse files
new method to construct an SO3 object (#159)
Co-authored-by: Mark Yeatman <129521731+myeatman-bdai@users.noreply.github.com>
1 parent a08f25e commit 659ba24
Copy full SHA for 659ba24

File tree

Expand file treeCollapse file tree

2 files changed

+54
-0
lines changed
Filter options
Expand file treeCollapse file tree

2 files changed

+54
-0
lines changed

‎spatialmath/pose3d.py

Copy file name to clipboardExpand all lines: spatialmath/pose3d.py
+42Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -725,6 +725,48 @@ def vval(v):
725725

726726
return cls(np.c_[x, y, z], check=True)
727727

728+
@classmethod
729+
def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self:
730+
"""
731+
Construct a new SO(3) from a vector and its rotated image
732+
733+
:param v1: initial vector
734+
:type v1: array_like(3)
735+
:param v2: vector after rotation
736+
:type v2: array_like(3)
737+
:param tol: tolerance for singularity in units of eps, defaults to 20
738+
:type tol: float
739+
:return: SO(3) rotation
740+
:rtype: :class:`SO3` instance
741+
742+
``SO3.RotatedVector(v1, v2)`` is an SO(3) rotation defined in terms of
743+
two vectors. The rotation takes vector ``v1`` to ``v2``.
744+
745+
.. runblock:: pycon
746+
747+
>>> from spatialmath import SO3
748+
>>> v1 = [1, 2, 3]
749+
>>> v2 = SO3.Eul(0.3, 0.4, 0.5) * v1
750+
>>> print(v2)
751+
>>> R = SO3.RotatedVector(v1, v2)
752+
>>> print(R)
753+
>>> print(R * v1)
754+
755+
.. note:: The vectors do not have to be unit-length.
756+
"""
757+
# https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
758+
v1 = smb.unitvec(v1)
759+
v2 = smb.unitvec(v2)
760+
v = smb.cross(v1, v2)
761+
s = smb.norm(v)
762+
if abs(s) < tol * np.finfo(float).eps:
763+
return cls(np.eye(3), check=False)
764+
else:
765+
c = np.dot(v1, v2)
766+
V = smb.skew(v)
767+
R = np.eye(3) + V + V @ V * (1 - c) / (s**2)
768+
return cls(R, check=False)
769+
728770
@classmethod
729771
def AngleAxis(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> Self:
730772
r"""

‎tests/test_pose3d.py

Copy file name to clipboardExpand all lines: tests/test_pose3d.py
+12Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -716,6 +716,17 @@ def test_functions_lie(self):
716716
nt.assert_equal(R, SO3.EulerVec(R.eulervec()))
717717
np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3))
718718

719+
720+
def test_rotatedvector(self):
721+
v1 = [1, 2, 3]
722+
R = SO3.Eul(0.3, 0.4, 0.5)
723+
v2 = R * v1
724+
Re = SO3.RotatedVector(v1, v2)
725+
np.testing.assert_almost_equal(v2, Re * v1)
726+
727+
Re = SO3.RotatedVector(v1, v1)
728+
np.testing.assert_almost_equal(Re, np.eye(3))
729+
719730
R = SO3() # identity matrix case
720731

721732
# Check log and exponential map
@@ -748,6 +759,7 @@ def test_mean(self):
748759
array_compare(m, SO3.RPY(0.1, 0.2, 0.3))
749760

750761

762+
751763
# ============================== SE3 =====================================#
752764

753765

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.