Repository: dfki-ric/pytransform3d (language: python)

Issue:
Batch inversion of transformation matrices
The following code should be correct and go to pytransform3d.trajectories. It is not tested though.

```python
def batch_invert_transform(A2Bs):
    """Invert transforms.

    Parameters
    ----------
    A2Bs : array-like, shape (..., 4, 4)
        Transforms from frames A to frames B

    Returns
    -------
    B2As : array-like, shape (..., 4, 4)
        Transforms from frames B to frames A
    """
    A2Bs = np.asarray(A2Bs)
    instances_shape = A2Bs.shape[:-2]
    B2As = np.empty_like(A2Bs)
    # ( R t )^-1   ( R^T -R^T*t )
    # ( 0 1 )    = ( 0    1     )
    B2As[..., :3, :3] = A2Bs[..., :3, :3].transpose(list(range(A2Bs.ndim - 2)) + [A2Bs.ndim - 1, A2Bs.ndim - 2])
    B2As[..., :3, 3] = np.einsum(
        "nij,nj->ni",
        -B2As[..., :3, :3].reshape(-1, 3, 3),
        A2Bs[..., :3, 3].reshape(-1, 3)).reshape(
        *(list(instances_shape) + [3]))
    B2As[..., 3, :3] = 0.0
    B2As[..., 3, 3] = 1.0
    return B2As
```


Discussion / hints:

Fix patch:
diff --git a/doc/source/api.rst b/doc/source/api.rst
index 8afb5a40d..62f471b38 100644
--- a/doc/source/api.rst
+++ b/doc/source/api.rst
@@ -370,6 +370,7 @@ Testing
    :toctree: _apidoc/
    :template: function.rst
 
+   ~pytransform3d.trajectories.invert_transforms
    ~pytransform3d.trajectories.transforms_from_pqs
    ~pytransform3d.trajectories.pqs_from_transforms
    ~pytransform3d.trajectories.exponential_coordinates_from_transforms
diff --git a/pytransform3d/trajectories.py b/pytransform3d/trajectories.py
index 2ed04be9b..18be42c85 100644
--- a/pytransform3d/trajectories.py
+++ b/pytransform3d/trajectories.py
@@ -12,6 +12,36 @@
 from .transformations import transform_from_exponential_coordinates
 
 
+def invert_transforms(A2Bs):
+    """Invert transforms.
+
+    Parameters
+    ----------
+    A2Bs : array-like, shape (..., 4, 4)
+        Transforms from frames A to frames B
+
+    Returns
+    -------
+    B2As : array, shape (..., 4, 4)
+        Transforms from frames B to frames A
+    """
+    A2Bs = np.asarray(A2Bs)
+    instances_shape = A2Bs.shape[:-2]
+    B2As = np.empty_like(A2Bs)
+    # ( R t )^-1   ( R^T -R^T*t )
+    # ( 0 1 )    = ( 0    1     )
+    B2As[..., :3, :3] = A2Bs[..., :3, :3].transpose(
+        list(range(A2Bs.ndim - 2)) + [A2Bs.ndim - 1, A2Bs.ndim - 2])
+    B2As[..., :3, 3] = np.einsum(
+        "nij,nj->ni",
+        -B2As[..., :3, :3].reshape(-1, 3, 3),
+        A2Bs[..., :3, 3].reshape(-1, 3)).reshape(
+        *(list(instances_shape) + [3]))
+    B2As[..., 3, :3] = 0.0
+    B2As[..., 3, 3] = 1.0
+    return B2As
+
+
 def transforms_from_pqs(P, normalize_quaternions=True):
     """Get sequence of homogeneous matrices from positions and quaternions.
 
diff --git a/pytransform3d/trajectories.pyi b/pytransform3d/trajectories.pyi
index 7dd883700..a10c984be 100644
--- a/pytransform3d/trajectories.pyi
+++ b/pytransform3d/trajectories.pyi
@@ -4,6 +4,9 @@ from mpl_toolkits.mplot3d import Axes3D
 from typing import Union
 
 
+def invert_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ...
+
+
 def transforms_from_pqs(
         P: npt.ArrayLike, normalize_quaternions: bool = ...) -> np.ndarray: ...
 


Test patch:
diff --git a/pytransform3d/test/test_trajectories.py b/pytransform3d/test/test_trajectories.py
index 266a4f907..2ecb1d1bb 100644
--- a/pytransform3d/test/test_trajectories.py
+++ b/pytransform3d/test/test_trajectories.py
@@ -1,6 +1,6 @@
 import numpy as np
 from pytransform3d.trajectories import (
-    transforms_from_pqs, pqs_from_transforms,
+    invert_transforms, transforms_from_pqs, pqs_from_transforms,
     transforms_from_exponential_coordinates,
     exponential_coordinates_from_transforms,
     pqs_from_dual_quaternions, dual_quaternions_from_pqs,
@@ -13,11 +13,39 @@
     exponential_coordinates_from_transform, translate_transform,
     rotate_transform, random_transform, transform_from_pq,
     concatenate_dual_quaternions, dq_prod_vector,
-    assert_unit_dual_quaternion_equal)
+    assert_unit_dual_quaternion_equal, invert_transform)
 from pytransform3d.batch_rotations import norm_vectors
 from numpy.testing import assert_array_almost_equal
 
 
+def test_invert_transforms_0dims():
+    random_state = np.random.RandomState(0)
+    A2B = random_transform(random_state)
+    B2A = invert_transform(A2B)
+    assert_array_almost_equal(B2A, invert_transforms(A2B))
+
+
+def test_invert_transforms_1dims():
+    random_state = np.random.RandomState(1)
+    A2Bs = np.empty((3, 4, 4))
+    B2As = np.empty((3, 4, 4))
+    for i in range(len(A2Bs)):
+        A2Bs[i] = random_transform(random_state)
+        B2As[i] = invert_transform(A2Bs[i])
+    assert_array_almost_equal(B2As, invert_transforms(A2Bs))
+
+
+def test_invert_transforms_2dims():
+    random_state = np.random.RandomState(1)
+    A2Bs = np.empty((9, 4, 4))
+    B2As = np.empty((9, 4, 4))
+    for i in range(len(A2Bs)):
+        A2Bs[i] = random_transform(random_state)
+        B2As[i] = invert_transform(A2Bs[i])
+    assert_array_almost_equal(
+        B2As.reshape(3, 3, 4, 4), invert_transforms(A2Bs.reshape(3, 3, 4, 4)))
+
+
 def test_transforms_from_pqs_0dims():
     random_state = np.random.RandomState(0)
     pq = np.empty(7)