Merge pull request #85 from bdaiinstitute/bokorn-bdaii/orthoginalize_TwoVector_axes

Added orthogonalization to TwoVectors w unit tests

Author: myeatman-bdai

spatialmath/base/vectors.py

@@ -789,6 +789,40 @@ def removesmall(v: ArrayLike, tol: float = 100) -> NDArray:
     return np.where(np.abs(v) < tol * _eps, 0, v)
 
 
+def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3:
+    """
+    Projects vector v1 onto v2. Returns a vector parallel to v2.
+
+    :param v1: vector to be projected
+    :type v1: array_like(n)
+    :param v2: vector to be projected onto
+    :type v2: array_like(n)
+    :return: vector projection of v1 onto v2 (parrallel to v2)
+    :rtype: ndarray(n)
+    """
+    return np.dot(v1, v2) * v2
+
+
+def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3:
+    """
+    Orthoginalizes vector v1 with respect to v2 with minimum rotation.
+    Returns a the nearest vector to v1 that is orthoginal to v2.
+
+    :param v1: vector to be orthoginalized
+    :type v1: array_like(n)
+    :param v2: vector that returned vector will be orthoginal to
+    :type v2: array_like(n)
+    :param normalize: whether to normalize the output vector
+    :type normalize: bool
+    :return: nearest vector to v1 that is orthoginal to v2
+    :rtype: ndarray(n)
+    """
+    v_orth = v1 - project(v1, v2)
+    if normalize:
+        v_orth = v_orth / np.linalg.norm(v_orth)
+    return v_orth
+
+
 if __name__ == "__main__":  # pragma: no cover
     import pathlib
 

spatialmath/pose3d.py

@@ -28,6 +28,7 @@
 
 import spatialmath.base as smb
 from spatialmath.base.types import *
+from spatialmath.base.vectors import orthogonalize
 from spatialmath.baseposematrix import BasePoseMatrix
 from spatialmath.pose2d import SE2
 
@@ -651,8 +652,10 @@ def TwoVectors(
         axes in terms of the old axes.  Axes are denoted by strings ``"x"``,
         ``"y"``, ``"z"``, ``"-x"``, ``"-y"``, ``"-z"``.
 
-        The directions can also be specified by 3-element vectors, but these
-        must be orthogonal.
+        The directions can also be specified by 3-element vectors. If the vectors are not orthogonal,
+        they will orthogonalized w.r.t. the first available dimension. I.e. if x is available, it will be
+        normalized and the remaining vector will be orthogonalized w.r.t. x, else, y will be normalized
+        and z will be orthogonalized w.r.t. y.
 
         To create a rotation where the new frame has its x-axis in -z-direction
         of the previous frame, and its z-axis in the x-direction of the previous
@@ -679,25 +682,41 @@ def vval(v):
             else:
                 return smb.unitvec(smb.getvector(v, 3))
 
-        if x is not None and y is not None and z is None:
+        if x is not None and y is not None and z is not None:
+            raise ValueError(
+                "Only two vectors should be provided. Please set one to None."
+            )
+
+        elif x is not None and y is not None and z is None:
             # z = x x y
             x = vval(x)
             y = vval(y)
+            # Orthogonalizes y w.r.t. x
+            y = orthogonalize(y, x, normalize=True)
             z = np.cross(x, y)
 
         elif x is None and y is not None and z is not None:
             # x = y x z
             y = vval(y)
             z = vval(z)
+            # Orthogonalizes z w.r.t. y
+            z = orthogonalize(z, y, normalize=True)
             x = np.cross(y, z)
 
         elif x is not None and y is None and z is not None:
             # y = z x x
             z = vval(z)
             x = vval(x)
+            # Orthogonalizes z w.r.t. x
+            z = orthogonalize(z, x, normalize=True)
             y = np.cross(z, x)
 
-        return cls(np.c_[x, y, z], check=False)
+        else:
+            raise ValueError(
+                "Insufficient number of vectors. Please provide exactly two vectors."
+            )
+
+        return cls(np.c_[x, y, z], check=True)
 
     @classmethod
     def AngleAxis(cls, theta: float, v: ArrayLike3, *, unit: str = "rad") -> Self:

tests/test_pose3d.py

@@ -216,7 +216,32 @@ def test_constructor_AngVec(self):
         array_compare(R, roty(0.3))
         self.assertIsInstance(R, SO3)
 
+    def test_constructor_TwoVec(self):
+        # Randomly selected vectors
+        v1 = [1, 73, -42]
+        v2 = [0, 0.02, 57]
+        v3 = [-2, 3, 9]
+
+        # x and y given
+        R = SO3.TwoVectors(x=v1, y=v2)
+        self.assertIsInstance(R, SO3)
+        nt.assert_almost_equal(np.linalg.det(R), 1, 5)
+        # x axis should equal normalized x vector
+        nt.assert_almost_equal(R.R[:, 0], v1 / np.linalg.norm(v1), 5)
+
+        # y and z given
+        R = SO3.TwoVectors(y=v2, z=v3)
+        self.assertIsInstance(R, SO3)
+        nt.assert_almost_equal(np.linalg.det(R), 1, 5)
+        # y axis should equal normalized y vector
+        nt.assert_almost_equal(R.R[:, 1], v2 / np.linalg.norm(v2), 5)
 
+        # x and z given
+        R = SO3.TwoVectors(x=v3, z=v1)
+        self.assertIsInstance(R, SO3)
+        nt.assert_almost_equal(np.linalg.det(R), 1, 5)
+        # x axis should equal normalized x vector
+        nt.assert_almost_equal(R.R[:, 0], v3 / np.linalg.norm(v3), 5)
 
     def test_shape(self):
         a = SO3()