new method to construct an SO3 object (bdaiinstitute#159)

petercorke · myeatman-bdai · web-flow · commit 659ba24a80cc · 2025-01-30T12:26:47.000-05:00
Co-authored-by: Mark Yeatman &lt;129521731+myeatman-bdai@users.noreply.github.com&gt;
diff --git 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
@@ -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 =====================================#
 
 
