diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index b42d752c..3b4821e5 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -725,6 +725,48 @@ def vval(v): return cls(np.c_[x, y, z], check=True) + @classmethod + def RotatedVector(cls, v1: ArrayLike3, v2: ArrayLike3, tol=20) -> Self: + """ + Construct a new SO(3) from a vector and its rotated image + + :param v1: initial vector + :type v1: array_like(3) + :param v2: vector after rotation + :type v2: array_like(3) + :param tol: tolerance for singularity in units of eps, defaults to 20 + :type tol: float + :return: SO(3) rotation + :rtype: :class:`SO3` instance + + ``SO3.RotatedVector(v1, v2)`` is an SO(3) rotation defined in terms of + two vectors. The rotation takes vector ``v1`` to ``v2``. + + .. runblock:: pycon + + >>> from spatialmath import SO3 + >>> v1 = [1, 2, 3] + >>> v2 = SO3.Eul(0.3, 0.4, 0.5) * v1 + >>> print(v2) + >>> R = SO3.RotatedVector(v1, v2) + >>> print(R) + >>> print(R * v1) + + .. note:: The vectors do not have to be unit-length. + """ + # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d + v1 = smb.unitvec(v1) + v2 = smb.unitvec(v2) + v = smb.cross(v1, v2) + s = smb.norm(v) + if abs(s) < tol * np.finfo(float).eps: + return cls(np.eye(3), check=False) + else: + c = np.dot(v1, v2) + V = smb.skew(v) + R = np.eye(3) + V + V @ V * (1 - c) / (s**2) + return cls(R, check=False) + @classmethod def AngleAxis(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> Self: r""" diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 86d4c414..58396441 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -716,6 +716,17 @@ def test_functions_lie(self): nt.assert_equal(R, SO3.EulerVec(R.eulervec())) np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3)) + + def test_rotatedvector(self): + v1 = [1, 2, 3] + R = SO3.Eul(0.3, 0.4, 0.5) + v2 = R * v1 + Re = SO3.RotatedVector(v1, v2) + np.testing.assert_almost_equal(v2, Re * v1) + + Re = SO3.RotatedVector(v1, v1) + np.testing.assert_almost_equal(Re, np.eye(3)) + R = SO3() # identity matrix case # Check log and exponential map @@ -748,6 +759,7 @@ def test_mean(self): array_compare(m, SO3.RPY(0.1, 0.2, 0.3)) + # ============================== SE3 =====================================#