Merge pull request #83 from bdaiinstitute/add-copy-when-passed-as-input

[SW-235] Added a copy_from method to copy inputs instead of just passing by re…

Author: myeatman-bdai

spatialmath/pose3d.py

@@ -1934,6 +1934,27 @@ def Rt(
             t = np.zeros((3,))
         return cls(smb.rt2tr(R, t, check=check), check=check)
 
+    @classmethod
+    def CopyFrom(
+        cls,
+        T: SE3Array,
+        check: bool = True
+    ) -> SE3:
+        """
+        Create an SE(3) from a 4x4 numpy array that is passed by value.
+
+        :param T: homogeneous transformation
+        :type T: ndarray(4, 4)
+        :param check: check rotation validity, defaults to True
+        :type check: bool, optional
+        :raises ValueError: bad rotation matrix, bad transformation matrix
+        :return: SE(3) matrix representing that transformation
+        :rtype: SE3 instance
+        """
+        if T is None:
+            raise ValueError("Transformation matrix must not be None")
+        return cls(np.copy(T), check=check)
+
     def angdist(self, other: SE3, metric: int = 6) -> float:
         r"""
         Angular distance metric between poses

tests/test_pose3d.py

@@ -888,6 +888,21 @@ def test_properties(self):
         nt.assert_equal(R.N, 3)
         nt.assert_equal(R.shape, (4, 4))
 
+        # Testing the CopyFrom function
+        mutable_array = np.eye(4)
+        pass_by_ref = SE3(mutable_array)
+        pass_by_val = SE3.CopyFrom(mutable_array)
+        mutable_array[0, 3] = 5.0
+        nt.assert_allclose(pass_by_val.data[0], np.eye(4))
+        nt.assert_allclose(pass_by_ref.data[0], mutable_array)
+        nt.assert_raises(
+            AssertionError,
+            nt.assert_allclose,
+            pass_by_val.data[0],
+            pass_by_ref.data[0]
+            )
+
+
     def test_arith(self):
         T = SE3(1, 2, 3)