Accept a stacked (N, r, c) ndarray as a sequence of poses (SE3, SO3, SE2, SO2)

spatialmath/baseposelist.py

@@ -154,6 +154,8 @@ def arghandler(
 
         #. None, an identity value is created
         #. a numpy.ndarray of the appropriate shape and value which is valid for the subclass
+        #. a stacked numpy.ndarray of shape (N, r, c), where (r, c) is the shape of one value
+           of the subclass, which is a sequence of N values
         #. a list whose elements all meet the criteria above
         #. an instance of the subclass
         #. a list whose elements are all singelton instances of the subclass
@@ -187,6 +189,19 @@ def arghandler(
         elif isinstance(arg, np.ndarray):
             # it's a numpy array
 
+            shape = tuple(self.shape)
+            if len(shape) == 2 and arg.ndim == 3 and arg.shape[1:] == shape:
+                # a stacked (N, r, c) array is a sequence of N values
+                values = [self._import(x, check=check) for x in arg]
+                if any(x is None for x in values):
+                    return False
+                self.data = values
+                return True
+
+            if not check and arg.shape != shape:
+                # check=False skips the value checks, but not the shape check
+                return False
+
             x = self._import(arg, check=check)
             if x is not None:
                 self.data = [x]

spatialmath/pose3d.py

@@ -1096,6 +1096,9 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
         - ``SE3([X1, X2, ... XN])`` has ``N`` values
           given by the elements ``Xi`` each of which is an SE3 instance.
 
+        - ``SE3(A)`` where ``A`` is an ``(N, 4, 4)`` NumPy array has ``N`` values,
+          equivalent to ``SE3(list(A))``.
+
         :SymPy: supported
         """
         if y is None and z is None:
@@ -1115,7 +1118,10 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
                 self.data = [smb.transl(T) for T in x]
 
             else:
-                raise ValueError("bad argument to constructor")
+                raise ValueError(
+                    "bad argument to constructor: expected an SE3, SO3 or SE2 instance, a 4x4 or "
+                    "(N, 4, 4) array, a list of those, a 3-vector or an (N, 3) array of translations"
+                )
 
         elif y is not None and z is not None:
             # SE3(x, y, z)

tests/test_pose3d.py

@@ -784,6 +784,26 @@ class TestSE3(unittest.TestCase):
     def tearDownClass(cls):
         plt.close("all")
 
+    def test_constructor_stacked_array(self):
+        # an (N, 4, 4) ndarray is a sequence of N poses, like a list of 4x4 arrays
+        poses = [SE3.Rx(0.3), SE3(1, 2, 3) * SE3.Ry(0.4), SE3.Rz(-0.2)]
+        stacked = np.array([T.A for T in poses])
+        for check in (True, False):
+            T = SE3(stacked, check=check)
+            self.assertEqual(len(T), 3)
+            for k in range(3):
+                nt.assert_array_almost_equal(T[k].A, poses[k].A)
+        self.assertEqual(len(SE3(np.empty((0, 4, 4)))), 0)
+        self.assertEqual(len(SO3(np.array([SO3.Rx(0.1).A, SO3.Ry(0.2).A]))), 2)
+
+        # stacked matrices that are not valid poses are still rejected by check=True
+        with self.assertRaises(ValueError):
+            SE3(2 * stacked)
+        # and a stack of matrices of the wrong size is rejected whatever the value of check
+        for check in (True, False):
+            with self.assertRaises(ValueError):
+                SE3(np.ones((3, 3, 3)), check=check)
+
     def test_constructor(self):
         # null constructor
         R = SE3()