from __future__ import annotations
from collections.abc import Iterable, Iterator
from types import EllipsisType, ModuleType, NotImplementedType
from collections.abc import Callable
import numpy as np
from scipy._lib._array_api import (
array_namespace,
is_numpy,
ArrayLike,
Array,
xp_capabilities,
)
from scipy.spatial.transform import Rotation
from scipy.spatial.transform._rotation import _promote
import scipy.spatial.transform._rigid_transform_cy as cython_backend
import scipy.spatial.transform._rigid_transform_xp as xp_backend
import scipy._lib.array_api_extra as xpx
from scipy._lib.array_api_compat import device
from scipy._lib._array_api import xp_promote
from scipy._lib._util import broadcastable
__all__ = ["RigidTransform"]
backend_registry = {array_namespace(np.empty(0)): cython_backend}
def select_backend(xp: ModuleType, cython_compatible: bool):
"""Select the backend for the given array library.
We need this selection function because the Cython backend for numpy does not
support quaternions of arbitrary dimensions. We therefore only use the Array API
backend for numpy if we are dealing with rotations of more than one leading
dimension.
"""
if is_numpy(xp) and not cython_compatible:
return xp_backend
return backend_registry.get(xp, xp_backend)
@xp_capabilities()
def normalize_dual_quaternion(dual_quat: ArrayLike) -> Array:
"""Normalize dual quaternion."""
xp = array_namespace(dual_quat)
dual_quat = _promote(dual_quat, xp=xp)
single = dual_quat.ndim == 1 and is_numpy(xp)
if single:
dual_quat = xpx.atleast_nd(dual_quat, ndim=2, xp=xp)
cython_compatible = dual_quat.ndim < 3
dq = select_backend(xp, cython_compatible).normalize_dual_quaternion(dual_quat)
if single:
return dq[0]
return dq
class RigidTransform:
"""Rigid transform in 3 dimensions.
This class provides an interface to initialize from and represent rigid
transforms (rotation and translation) in 3D space. In different fields,
this type of transform may be referred to as "*pose*" (especially in
robotics), "*extrinsic parameters*", or the "*model matrix*" (especially in
computer graphics), but the core concept is the same: a rotation and
translation describing the orientation of one 3D coordinate frame relative
to another. Mathematically, these transforms belong to the Special
Euclidean group SE(3), which encodes rotation (SO(3)) plus translation.
The following operations on rigid transforms are supported:
- Application on vectors
- Transformation composition
- Transformation inversion
- Transformation indexing
Note that coordinate systems must be right-handed. Because of this, this
class more precisely represents *proper* rigid transformations in SE(3)
rather than rigid transforms in E(3) more generally [1]_.
Indexing within a transform is supported since multiple transforms can be
stored within a single `RigidTransform` instance.
Multiple transforms can be stored in a single `RigidTransform` object, which can be
initialized using N-dimensional arrays and supports broadcasting for all
operations.
To create `RigidTransform` objects use ``from_...`` methods (see examples
below). ``RigidTransform(...)`` is not supposed to be instantiated directly.
For rigorous introductions to rigid transforms, see [2]_, [3]_, and [4]_.
Attributes
----------
single
rotation
translation
Methods
-------
__len__
__getitem__
__mul__
__pow__
from_matrix
from_rotation
from_translation
from_components
from_exp_coords
from_dual_quat
as_matrix
as_components
as_exp_coords
as_dual_quat
concatenate
mean
apply
inv
identity
References
----------
.. [1] https://en.wikipedia.org/wiki/Rigid_transformation
.. [2] https://motion.cs.illinois.edu/RoboticSystems/CoordinateTransformations.html
.. [3] https://www.brainvoyager.com/bv/doc/UsersGuide/CoordsAndTransforms/SpatialTransformationMatrices.html
.. [4] Kevin M. Lynch and Frank C. Park, "Modern Robotics: Mechanics,
Planning, and Control" Chapter 3.3, 2017, Cambridge University Press.
https://hades.mech.northwestern.edu/images/2/25/MR-v2.pdf#page=107.31
.. [5] Paul Furgale, "Representing Robot Pose: The good, the bad, and the
ugly", June 9, 2014.
https://rpg.ifi.uzh.ch/docs/teaching/2024/FurgaleTutorial.pdf
Notes
-----
.. versionadded:: 1.16.0
Examples
--------
A `RigidTransform` instance can be initialized in any of the above formats
and converted to any of the others. The underlying object is independent of
the representation used for initialization.
**Notation Conventions and Composition**
The notation here largely follows the convention defined in [5]_. When we
name transforms, we read the subscripts from right to left. So ``tf_A_B``
represents a transform A <- B and can be interpreted as:
- the coordinates and orientation of B relative to A
- the transformation of points from B to A
- the pose of B described in A's coordinate system
.. parsed-literal::
:class: highlight-none
tf_A_B
^ ^
| |
| --- from B
|
----- to A
When composing transforms, the order is important. Transforms are not
commutative, so in general ``tf_A_B * tf_B_C`` is not the same as
``tf_B_C * tf_A_B``. Transforms are composed and applied to vectors
right-to-left. So ``(tf_A_B * tf_B_C).apply(p_C)`` is the same as
``tf_A_B.apply(tf_B_C.apply(p_C))``.
When composed, transforms should be ordered such that the multiplication
operator is surrounded by a single frame, so the frame "cancels out" and
the outside frames are left. In the example below, B cancels out and the
outside frames A and C are left. Or to put it another way, A <- C is the
same as A <- B <- C.
.. parsed-literal::
:class: highlight-none
----------- B cancels out
| |
v v
tf_A_C = tf_A_B * tf_B_C
^ ^
| |
------------ to A, from C are left
When we notate vectors, we write the subscript of the frame that the vector
is defined in. So ``p_B`` means the point ``p`` defined in frame B. To
transform this point from frame B to coordinates in frame A, we apply the
transform ``tf_A_B`` to the vector, lining things up such that the notated
B frames are next to each other and "cancel out".
.. parsed-literal::
:class: highlight-none
------------ B cancels out
| |
v v
p_A = tf_A_B.apply(p_B)
^
|
-------------- A is left
**Visualization**
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
The following function can be used to plot transforms with Matplotlib
by showing how they transform the standard x, y, z coordinate axes:
>>> import matplotlib.pyplot as plt
>>> colors = ("#FF6666", "#005533", "#1199EE") # Colorblind-safe RGB
>>> def plot_transformed_axes(ax, tf, name=None, scale=1):
... r = tf.rotation
... t = tf.translation
... loc = np.array([t, t])
... for i, (axis, c) in enumerate(zip((ax.xaxis, ax.yaxis, ax.zaxis),
... colors)):
... axlabel = axis.axis_name
... axis.set_label_text(axlabel)
... axis.label.set_color(c)
... axis.line.set_color(c)
... axis.set_tick_params(colors=c)
... line = np.zeros((2, 3))
... line[1, i] = scale
... line_rot = r.apply(line)
... line_plot = line_rot + loc
... ax.plot(line_plot[:, 0], line_plot[:, 1], line_plot[:, 2], c)
... text_loc = line[1]*1.2
... text_loc_rot = r.apply(text_loc)
... text_plot = text_loc_rot + t
... ax.text(*text_plot, axlabel.upper(), color=c,
... va="center", ha="center")
... ax.text(*tf.translation, name, color="k", va="center", ha="center",
... bbox={"fc": "w", "alpha": 0.8, "boxstyle": "circle"})
**Defining Frames**
Let's work through an example.
First, define the "world frame" A, also called the "base frame".
All frames are the identity transform from their own perspective.
>>> tf_A = Tf.identity()
We will visualize a new frame B in A's coordinate system. So we need to
define the transform that converts coordinates from frame B to frame A
(A <- B).
Physically, let's imagine constructing B from A by:
1) Rotating A by +90 degrees around its x-axis.
2) Translating the rotated frame 2 units in A's -x direction.
From A's perspective, B is at [-2, 0, 0] and rotated +90 degrees about the
x-axis, which is exactly the transform A <- B.
>>> t_A_B = np.array([-2, 0, 0])
>>> r_A_B = R.from_euler('xyz', [90, 0, 0], degrees=True)
>>> tf_A_B = Tf.from_components(t_A_B, r_A_B)
Let's plot these frames.
>>> fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
>>> plot_transformed_axes(ax, tf_A, name="tfA") # A plotted in A
>>> plot_transformed_axes(ax, tf_A_B, name="tfAB") # B plotted in A
>>> ax.set_title("A, B frames with respect to A")
>>> ax.set_aspect("equal")
>>> ax.figure.set_size_inches(6, 5)
>>> plt.show()
Now let's visualize a new frame C in B's coordinate system.
Let's imagine constructing C from B by:
1) Translating B by 2 units in its +z direction.
2) Rotating B by +30 degrees around its z-axis.
>>> t_B_C = np.array([0, 0, 2])
>>> r_B_C = R.from_euler('xyz', [0, 0, 30], degrees=True)
>>> tf_B_C = Tf.from_components(t_B_C, r_B_C)
In order to plot these frames from a consistent perspective, we need to
calculate the transform between A and C. Note that we do not make this
transform directly, but instead compose intermediate transforms that let us
get from C to A:
>>> tf_A_C = tf_A_B * tf_B_C # A <- B <- C
Now we can plot these three frames from A's perspective.
>>> fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
>>> plot_transformed_axes(ax, tf_A, name="tfA") # A plotted in A
>>> plot_transformed_axes(ax, tf_A_B, name="tfAB") # B plotted in A
>>> plot_transformed_axes(ax, tf_A_C, name="tfAC") # C plotted in A
>>> ax.set_title("A, B, C frames with respect to A")
>>> ax.set_aspect("equal")
>>> ax.figure.set_size_inches(6, 5)
>>> plt.show()
**Transforming Vectors**
Let's transform a vector from A, to B and C. To do this, we will first
invert the transforms we already have from B and C, to A.
>>> tf_B_A = tf_A_B.inv() # B <- A
>>> tf_C_A = tf_A_C.inv() # C <- A
Now we can define a point in A and use the above transforms to get its
coordinates in B and C:
>>> p1_A = np.array([1, 0, 0]) # +1 in x_A direction
>>> p1_B = tf_B_A.apply(p1_A)
>>> p1_C = tf_C_A.apply(p1_A)
>>> print(p1_A) # Original point 1 in A
[1 0 0]
>>> print(p1_B) # Point 1 in B
[3. 0. 0.]
>>> print(p1_C) # Point 1 in C
[ 2.59807621 -1.5 -2. ]
We can also do the reverse. We define a point in C and transform it to A:
>>> p2_C = np.array([0, 1, 0]) # +1 in y_C direction
>>> p2_A = tf_A_C.apply(p2_C)
>>> print(p2_C) # Original point 2 in C
[0 1 0]
>>> print(p2_A) # Point 2 in A
[-2.5 -2. 0.8660254]
Plot the frames with respect to A again, but also plot these two points:
>>> fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
>>> plot_transformed_axes(ax, tf_A, name="tfA") # A plotted in A
>>> plot_transformed_axes(ax, tf_A_B, name="tfAB") # B plotted in A
>>> plot_transformed_axes(ax, tf_A_C, name="tfAC") # C plotted in A
>>> ax.scatter(p1_A[0], p1_A[1], p1_A[2], color=colors[0]) # +1 x_A
>>> ax.scatter(p2_A[0], p2_A[1], p2_A[2], color=colors[1]) # +1 y_C
>>> ax.set_title("A, B, C frames and points with respect to A")
>>> ax.set_aspect("equal")
>>> ax.figure.set_size_inches(6, 5)
>>> plt.show()
**Switching Base Frames**
Up to this point, we have been visualizing frames from A's perspective.
Let's use the transforms we defined to visualize the frames from C's
perspective.
Now C is the "base frame" or "world frame". All frames are the identity
transform from their own perspective.
>>> tf_C = Tf.identity()
We've already defined the transform C <- A, and can obtain C <- B by
inverting the existing transform B <- C.
>>> tf_C_B = tf_B_C.inv() # C <- B
This lets us plot everything from C's perspective:
>>> fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
>>> plot_transformed_axes(ax, tf_C, name="tfC") # C plotted in C
>>> plot_transformed_axes(ax, tf_C_B, name="tfCB") # B plotted in C
>>> plot_transformed_axes(ax, tf_C_A, name="tfCA") # A plotted in C
>>> ax.scatter(p1_C[0], p1_C[1], p1_C[2], color=colors[0])
>>> ax.scatter(p2_C[0], p2_C[1], p2_C[2], color=colors[1])
>>> ax.set_title("A, B, C frames and points with respect to C")
>>> ax.set_aspect("equal")
>>> ax.figure.set_size_inches(6, 5)
>>> plt.show()
"""
def __init__(self, matrix: ArrayLike, normalize: bool = True, copy: bool = True):
"""Initialize from a 4x4 transformation matrix.
Rotations are not meant to be initialized directly. Please use one of
the `from_...` methods instead.
Parameters
----------
matrix : array_like, shape (..., 4, 4)
A single transformation matrix or a stack of transformation
matrices.
normalize : bool, optional
If True, orthonormalize the rotation matrix using singular value
decomposition. If False, the rotation matrix is not checked for
orthogonality or right-handedness.
copy : bool, optional
If True, copy the input matrix. If False, a reference to the input
matrix is used. If normalize is True, the input matrix is always
copied regardless of the value of copy.
Returns
-------
transform : `RigidTransform` instance
"""
xp = array_namespace(matrix)
self._xp = xp
matrix = _promote(matrix, xp=xp)
if matrix.shape[-2:] != (4, 4):
raise ValueError(
f"Expected `matrix` to have shape (..., 4, 4), got {matrix.shape}."
)
# We only need the _single flag for the cython backend. The Array API backend
# uses broadcasting by default and hence returns the correct shape without
# additional _single logic
self._single = matrix.ndim == 2 and is_numpy(xp)
if self._single:
matrix = xpx.atleast_nd(matrix, ndim=3, xp=xp)
self._backend = select_backend(xp, matrix.ndim < 4)
self._matrix = self._backend.from_matrix(matrix, normalize, copy)
def __repr__(self):
m = f"{self.as_matrix()!r}".splitlines()
# bump indent (+27 characters)
m[1:] = [" " * 27 + m[i] for i in range(1, len(m))]
return "RigidTransform.from_matrix(" + "\n".join(m) + ")"
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_matrix(matrix: ArrayLike) -> RigidTransform:
"""Initialize from a 4x4 transformation matrix.
Parameters
----------
matrix : array_like, shape (..., 4, 4)
Transformation matrices. Each matrix[..., :, :] represents a 4x4
transformation matrix.
Returns
-------
transform : `RigidTransform` instance
Notes
-----
4x4 rigid transformation matrices are of the form::
[ tx]
[ R ty]
[ tz]
[ 0 0 0 1]
where ``R`` is a 3x3 rotation matrix and ``t = [tx, ty, tz]`` is a 3x1
translation vector. As rotation matrices must be proper orthogonal, the
rotation component is orthonormalized using singular value decomposition
before initialization.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Creating a transform from a single matrix:
>>> m = np.array([[0, 1, 0, 2],
... [0, 0, 1, 3],
... [1, 0, 0, 4],
... [0, 0, 0, 1]])
>>> tf = Tf.from_matrix(m)
>>> tf.as_matrix()
array([[0., 1., 0., 2.],
[0., 0., 1., 3.],
[1., 0., 0., 4.],
[0., 0., 0., 1.]])
>>> tf.single
True
Creating a transform from an N-dimensional array of matrices:
>>> m = np.tile(np.eye(4), (2, 5, 1, 1)) # Shape (2, 5, 4, 4)
>>> tf = Tf.from_matrix(m)
>>> tf.shape
(2, 5)
>>> tf.single
False
>>> len(tf)
2
Matrices with a rotation component that is not proper orthogonal are
orthogonalized using singular value decomposition before initialization:
>>> tf = Tf.from_matrix(np.diag([2, 2, 2, 1]))
>>> tf.as_matrix()
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
"""
return RigidTransform(matrix, normalize=True, copy=True)
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_rotation(rotation: Rotation) -> RigidTransform:
"""Initialize from a rotation, without a translation.
When applying this transform to a vector ``v``, the result is the
same as if the rotation was applied to the vector.
``Tf.from_rotation(r).apply(v) == r.apply(v)``
Parameters
----------
rotation : `Rotation` instance
A single rotation or a rotation with N leading dimensions.
Returns
-------
transform : `RigidTransform` instance
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Creating a transform from a single rotation:
>>> r = R.from_euler("ZYX", [90, 30, 0], degrees=True)
>>> r.apply([1, 0, 0])
array([0. , 0.8660254, -0.5 ])
>>> tf = Tf.from_rotation(r)
>>> tf.apply([1, 0, 0])
array([0. , 0.8660254, -0.5 ])
>>> tf.single
True
The upper 3x3 submatrix of the transformation matrix is the rotation
matrix:
>>> np.allclose(tf.as_matrix()[:3, :3], r.as_matrix(), atol=1e-12)
True
Creating multiple transforms from a rotation with N leading dimensions:
>>> r = R.from_euler("ZYX", [[90, 30, 0], [45, 30, 60]], degrees=True)
>>> r.apply([1, 0, 0])
array([[0. , 0.8660254 , -0.5 ],
[0.61237244, 0.61237244, -0.5 ]])
>>> tf = Tf.from_rotation(r)
>>> tf.apply([1, 0, 0])
array([[0. , 0.8660254 , -0.5 ],
[0.61237244, 0.61237244, -0.5 ]])
>>> tf.single
False
>>> len(tf)
2
"""
if not isinstance(rotation, Rotation):
raise TypeError(
"Expected `rotation` to be a `Rotation` instance, "
f"got {type(rotation)}."
)
quat = rotation.as_quat()
xp = array_namespace(quat)
backend = select_backend(xp, quat.ndim < 3)
matrix = backend.from_rotation(quat)
return RigidTransform._from_raw_matrix(matrix, xp, backend)
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_translation(translation: ArrayLike) -> RigidTransform:
"""Initialize from a translation numpy array, without a rotation.
When applying this transform to a vector ``v``, the result is the same
as if the translation and vector were added together. If ``t`` is the
displacement vector of the translation, then:
``Tf.from_translation(t).apply(v) == t + v``
Parameters
----------
translation : array_like, shape (..., 3)
Translation vectors. Each translation[..., :] represents a 3D
translation vector.
Returns
-------
transform : `RigidTransform` instance
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Creating a transform from a single translation vector:
>>> t = np.array([2, 3, 4])
>>> t + np.array([1, 0, 0])
array([3, 3, 4])
>>> tf = Tf.from_translation(t)
>>> tf.apply([1, 0, 0])
array([3., 3., 4.])
>>> tf.single
True
The top 3x1 points in the rightmost column of the transformation matrix
is the translation vector:
>>> tf.as_matrix()
array([[1., 0., 0., 2.],
[0., 1., 0., 3.],
[0., 0., 1., 4.],
[0., 0., 0., 1.]])
>>> np.allclose(tf.as_matrix()[:3, 3], t)
True
Creating multiple transforms from an N-dimensional array of translation
vectors:
>>> t = np.array([[2, 3, 4], [1, 0, 0]])
>>> t + np.array([1, 0, 0])
array([[3, 3, 4],
[2, 0, 0]])
>>> tf = Tf.from_translation(t)
>>> tf.apply([1, 0, 0])
array([[3., 3., 4.],
[2., 0., 0.]])
>>> np.allclose(tf.as_matrix()[:, :3, 3], t)
True
>>> tf.single
False
>>> len(tf)
2
"""
xp = array_namespace(translation)
translation = _promote(translation, xp=xp)
backend = select_backend(xp, translation.ndim < 3)
matrix = backend.from_translation(translation)
return RigidTransform._from_raw_matrix(matrix, xp, backend)
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_components(translation: ArrayLike, rotation: Rotation) -> RigidTransform:
"""Initialize a rigid transform from translation and rotation
components.
When creating a rigid transform from a translation and rotation, the
translation is applied after the rotation, such that
``tf = Tf.from_components(translation, rotation)``
is equivalent to
``tf = Tf.from_translation(translation) * Tf.from_rotation(rotation)``.
When applying a transform to a vector ``v``, the result is the
same as if the transform was applied to the vector in the
following way: ``tf.apply(v) == translation + rotation.apply(v)``
Parameters
----------
translation : array_like, shape (..., 3)
Translation vectors. Each translation[..., :] represents a 3D
translation vector.
rotation : `Rotation` instance
Rotation objects. The shape must be broadcastable with the
translation shape.
Returns
-------
transform : `RigidTransform` instance
Rigid transform objects. The shape is determined by broadcasting
the translation and rotation shapes together.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Creating from a single rotation and translation:
>>> t = np.array([2, 3, 4])
>>> r = R.from_euler("ZYX", [90, 30, 0], degrees=True)
>>> r.as_matrix()
array([[ 0. , -1., 0. ],
[ 0.8660254, 0., 0.5 ],
[-0.5 , 0., 0.8660254 ]])
>>> tf = Tf.from_components(t, r)
>>> tf.rotation.as_matrix()
array([[ 0. , -1., 0. ],
[ 0.8660254, 0., 0.5 ],
[-0.5 , 0., 0.8660254 ]])
>>> tf.translation
array([2., 3., 4.])
>>> tf.single
True
When applying a transform to a vector ``v``, the result is the same as
if the transform was applied to the vector in the following way:
``tf.apply(v) == translation + rotation.apply(v)``
>>> r.apply([1, 0, 0])
array([0. , 0.8660254, -0.5 ])
>>> t + r.apply([1, 0, 0])
array([2. , 3.8660254, 3.5 ])
>>> tf.apply([1, 0, 0])
array([2. , 3.8660254, 3.5 ])
"""
rotation_tf = RigidTransform.from_rotation(rotation)
return RigidTransform.from_translation(translation) * rotation_tf
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_exp_coords(exp_coords: ArrayLike) -> RigidTransform:
r"""Initialize from exponential coordinates of transform.
This implements the exponential map that converts 6-dimensional real
vectors to SE(3).
An exponential coordinate vector consists of 6 elements
``[rx, ry, rz, vx, vy, vz]``. The first 3 encode rotation (and form a
rotation vector used in `Rotation.from_rotvec`) and the last 3 encode
translation (and form a translation vector for pure translations).
The exponential mapping can be expressed as matrix exponential
``T = exp(tau)``, where ``T`` is a 4x4 matrix representing a rigid
transform and ``tau`` is a 4x4 matrix formed from the elements of an
exponential coordinate vector::
tau = [ 0 -rz ry vx]
[ rz 0 -rx vy]
[-ry rx 0 vz]
[ 0 0 0 1]
Parameters
----------
exp_coords : array_like, shape (..., 6)
Exponential coordinate vectors. Each exp_coords[..., :] represents
a 6D exponential coordinate vector with the expected order of
components ``[rx, ry, rz, vx, vy, vz]``. The first 3 components
encode rotation and the last 3 encode translation.
Returns
-------
transform : `RigidTransform` instance
Rigid transform objects with the same leading dimensions as the input.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Creating from a single 6d vector of exponential coordinates:
>>> tf = Tf.from_exp_coords([
... -2.01041204, -0.52983629, 0.65773501,
... 0.10386614, 0.05855009, 0.54959179])
>>> tf.as_matrix()
array([[0.76406621, 0.10504613, -0.63652819, -0.10209961],
[0.59956454, -0.47987325, 0.64050295, 0.40158789],
[-0.2381705, -0.87102639, -0.42963687, 0.19637636],
[0., 0., 0., 1.]])
>>> tf.single
True
A vector of zeros represents the identity transform:
>>> tf = Tf.from_exp_coords(np.zeros(6))
>>> tf.as_matrix()
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
The last three numbers encode translation. If the first three numbers
are zero, the last three components can be interpreted as the
translation:
>>> tf_trans = Tf.from_exp_coords([0, 0, 0, 4.3, -2, 3.4])
>>> tf_trans.translation
array([4.3, -2., 3.4])
The first three numbers encode rotation as a rotation vector:
>>> tf_rot = Tf.from_exp_coords([0.5, 0.3, 0.1, 0, 0, 0])
>>> tf_rot.rotation.as_rotvec()
array([0.5, 0.3, 0.1])
Combining translation and rotation preserves the rotation vector,
but changes the last three components as they encode translation and
rotation:
>>> (tf_trans * tf_rot).as_exp_coords()
array([0.5, 0.3, 0.1, 3.64305882, -1.25879559, 4.46109265])
"""
xp = array_namespace(exp_coords)
exp_coords = xp_promote(exp_coords, force_floating=True, xp=xp)
if exp_coords.shape[-1] != 6:
raise ValueError(
f"Expected `exp_coords` to have shape (..., 6), got {exp_coords.shape}."
)
backend = select_backend(xp, cython_compatible=exp_coords.ndim < 3)
matrix = backend.from_exp_coords(exp_coords)
return RigidTransform._from_raw_matrix(matrix, xp, backend)
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def from_dual_quat(
dual_quat: ArrayLike, *, scalar_first: bool = False
) -> RigidTransform:
"""Initialize from a unit dual quaternion.
Unit dual quaternions encode orientation in a real unit quaternion
and translation in a dual quaternion. There is a double cover, i.e.,
the unit dual quaternions q and -q represent the same transform.
Unit dual quaternions must have a real quaternion with unit norm and
a dual quaternion that is orthogonal to the real quaternion to satisfy
the unit norm constraint. This function will enforce both properties
through normalization.
Parameters
----------
dual_quat : array_like, shape (..., 8)
Unit dual quaternions. Each dual_quat[..., :] represents a unit
dual quaternion. The real part is stored in the first four
components and the dual part in the last four components.
scalar_first : bool, optional
Whether the scalar component goes first or last in the two
individual quaternions that represent the real and the dual part.
Default is False, i.e. the scalar-last order is used.
Returns
-------
transform : `RigidTransform` instance
Rigid transform objects with the same leading dimensions as the input.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Creating from a single unit dual quaternion:
>>> tf = Tf.from_dual_quat([
... 0.0617101, -0.06483886, 0.31432811, 0.94508498,
... 0.04985168, -0.26119618, 0.1691491, -0.07743254])
>>> tf.as_matrix()
array([[0.79398752, -0.60213598, -0.08376202, 0.24605262],
[0.58613113, 0.79477941, -0.15740392, -0.4932833],
[0.16135089, 0.07588122, 0.98397557, 0.34262676],
[0., 0., 0., 1.]])
>>> tf.single
True
"""
xp = array_namespace(dual_quat)
dual_quat = _promote(dual_quat, xp=xp)
backend = select_backend(xp, dual_quat.ndim < 3)
matrix = backend.from_dual_quat(dual_quat, scalar_first=scalar_first)
return RigidTransform._from_raw_matrix(matrix, xp, backend)
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def identity(
num: int | None = None, *, shape: int | tuple[int, ...] | None = None
) -> RigidTransform:
"""Initialize an identity transform.
Composition with the identity transform has no effect, and
applying the identity transform to a vector has no effect.
Parameters
----------
num : int, optional
Number of identity transforms to generate. If None (default),
then a single transform is generated.
shape : int or tuple of ints, optional
Shape of the identity transforms. If specified, `num` must
be None.
Returns
-------
transform : `RigidTransform` instance
The identity transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Creating a single identity transform:
>>> tf = Tf.identity()
>>> tf.as_matrix()
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
>>> tf.single
True
The identity transform can be applied to a vector without effect:
>>> tf.apply([1, 2, 3])
array([1., 2., 3.])
The identity transform when composed with another transform has no
effect:
>>> rng = np.random.default_rng(123)
>>> t = rng.random(3)
>>> r = R.random(rng=rng)
>>> tf = Tf.from_components(t, r)
>>> np.allclose((Tf.identity() * tf).as_matrix(),
... tf.as_matrix(), atol=1e-12)
True
Multiple identity transforms can be generated at once:
>>> tf = Tf.identity(2)
>>> tf.as_matrix()
array([[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]])
>>> tf.single
False
>>> len(tf)
2
"""
if num is not None and shape is not None:
raise ValueError("Only one of `num` and `shape` can be specified.")
if num is None and shape is None:
shape = ()
elif num is not None:
shape = (num,)
elif isinstance(shape, int):
shape = (shape,)
elif not isinstance(shape, tuple):
raise ValueError("`shape` must be an int or a tuple of ints or None.")
matrix = np.tile(np.eye(4), shape + (1, 1))
# No need for a backend call here since identity is easy to construct and we are
# currently not offering a backend-specific identity matrix
return RigidTransform._from_raw_matrix(matrix, array_namespace(matrix))
@staticmethod
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def concatenate(
transforms: RigidTransform | Iterable[RigidTransform],
) -> RigidTransform:
"""
Concatenate a sequence of `RigidTransform` objects into a
single object.
Parameters
----------
transforms : sequence of `RigidTransform`
If a single `RigidTransform` instance is passed in, a copy of
it is returned.
Returns
-------
transform : `RigidTransform` instance
The concatenated transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> tf1 = Tf.from_translation([1, 0, 0])
>>> tf2 = Tf.from_translation([[2, 0, 0], [3, 0, 0]])
>>> Tf.concatenate([tf1, tf2]).translation
array([[1., 0., 0.],
[2., 0., 0.],
[3., 0., 0.]])
"""
if isinstance(transforms, RigidTransform):
return RigidTransform._from_raw_matrix(
transforms.as_matrix(), transforms._xp
)
if not all(isinstance(x, RigidTransform) for x in transforms):
raise TypeError("input must contain RigidTransform objects only")
xp = array_namespace(transforms[0].as_matrix())
matrix = xp.concat(
[xpx.atleast_nd(x.as_matrix(), ndim=3, xp=xp) for x in transforms]
)
return RigidTransform._from_raw_matrix(matrix, xp, None)
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def mean(self,
weights: ArrayLike | None = None,
axis: None | int | tuple[int, ...] = None
) -> RigidTransform:
"""Get the mean of the transforms.
The mean of a set of transforms is the same as the mean of its
rotation and translation components.
The mean used for the rotation component is the chordal L2 mean (also
called the projected or induced arithmetic mean) [1]_. If ``A`` is a
set of rotation matrices, then the mean ``M`` is the rotation matrix
that minimizes the following loss function:
.. math::
L(M) = \\sum_{i = 1}^{n} w_i \\lVert \\mathbf{A}_i -
\\mathbf{M} \\rVert^2 ,
where :math:`w_i`'s are the `weights` corresponding to each matrix.
Parameters
----------
weights : array_like shape (..., N), optional
Weights describing the relative importance of the transforms. If
None (default), then all values in `weights` are assumed to be
equal. If given, the shape of `weights` must be broadcastable to
the transform shape. Weights must be non-negative.
axis : None, int, or tuple of ints, optional
Axis or axes along which the means are computed. The default is to
compute the mean of all transforms.
Returns
-------
mean : `RigidTransform` instance
Single transform containing the mean of the transforms in the
current instance.
References
----------
.. [1] Hartley, Richard, et al.,
"Rotation Averaging", International Journal of Computer Vision
103, 2013, pp. 267-305.
Examples
--------
>>> import numpy as np
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> rng = np.random.default_rng(seed=123)
The mean of a set of transforms is the same as the mean of the
translation and rotation components:
>>> t = rng.random((4, 3))
>>> r = R.random(4, rng=rng)
>>> tf = Tf.from_components(t, r)
>>> tf.mean().as_matrix()
array([[ 0.61593485, -0.74508342, 0.25588075, 0.66999034],
[-0.59353615, -0.65246765, -0.47116962, 0.25481794],
[ 0.51801458, 0.13833531, -0.84411151, 0.52429339],
[0., 0., 0., 1.]])
>>> Tf.from_components(t.mean(axis=0), r.mean()).as_matrix()
array([[ 0.61593485, -0.74508342, 0.25588075, 0.66999034],
[-0.59353615, -0.65246765, -0.47116962, 0.25481794],
[ 0.51801458, 0.13833531, -0.84411151, 0.52429339],
[0., 0., 0., 1.]])
"""
mean = self._backend.mean(self._matrix, weights=weights, axis=axis)
return RigidTransform._from_raw_matrix(mean, xp=self._xp,
backend=self._backend)
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def as_matrix(self) -> Array:
"""Return a copy of the matrix representation of the transform.
4x4 rigid transformation matrices are of the form::
[ tx]
[ R ty]
[ tz]
[ 0 0 0 1]
where ``R`` is a 3x3 orthonormal rotation matrix and
``t = [tx, ty, tz]`` is a 3x1 translation vector.
Returns
-------
matrix : numpy.ndarray, shape (..., 4, 4)
Transformation matrices with the same leading dimensions as the transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
A transformation matrix is a 4x4 matrix formed from a 3x3 rotation
matrix and a 3x1 translation vector:
>>> t = np.array([2, 3, 4])
>>> r = R.from_matrix([[0, 0, 1],
... [1, 0, 0],
... [0, 1, 0]])
>>> tf = Tf.from_components(t, r)
>>> tf.as_matrix()
array([[ 0., 0., 1., 2.],
[ 1., 0., 0., 3.],
[ 0., 1., 0., 4.],
[ 0., 0., 0., 1.]])
>>> Tf.identity(2).as_matrix()
array([[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]])
"""
matrix = self._xp.asarray(self._matrix, copy=True)
if self._single:
return matrix[0, ...]
return matrix
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def as_components(self) -> tuple[Array, Rotation]:
"""Return the translation and rotation components of the transform,
where the rotation is applied first, followed by the translation.
4x4 rigid transformation matrices are of the form::
[ tx]
[ R ty]
[ tz]
[ 0 0 0 1]
Where ``R`` is a 3x3 orthonormal rotation matrix and
``t = [tx, ty, tz]`` is a 3x1 translation vector. This function
returns the rotation corresponding to this rotation matrix
``r = Rotation.from_matrix(R)`` and the translation vector ``t``.
When applying a transform ``tf`` to a vector ``v``, the result is the same
as if the rotation and translation components were applied to the vector
with the following operation:
``tf.apply(v) == translation + rotation.apply(v)``.
Returns
-------
translation : numpy.ndarray, shape (..., 3)
The translation of the transform.
rotation : `Rotation` instance
The rotation of the transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Recover the rotation and translation from a transform:
>>> t = np.array([2, 3, 4])
>>> r = R.from_matrix([[0, 0, 1],
... [1, 0, 0],
... [0, 1, 0]])
>>> tf = Tf.from_components(t, r)
>>> t, r = tf.as_components()
>>> t
array([2., 3., 4.])
>>> r.as_matrix()
array([[0., 0., 1.],
[1., 0., 0.],
[0., 1., 0.]])
The transform applied to a vector is equivalent to the rotation applied
to the vector, followed by the translation:
>>> t + r.apply([1, 0, 0])
array([2., 4., 4.])
>>> tf.apply([1, 0, 0])
array([2., 4., 4.])
"""
return self.translation, self.rotation
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def as_exp_coords(self) -> Array:
"""Return the exponential coordinates of the transform.
This implements the logarithmic map that converts SE(3) to 6-dimensional
real vectors.
This is an inverse of `from_exp_coords` where details on the mapping can
be found.
Returns
-------
exp_coords : numpy.ndarray, shape (..., 6)
Exponential coordinate vectors with the same leading dimensions as the
transform. The first three components define the rotation and the last
three components define the translation.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Get exponential coordinates of the identity matrix:
>>> Tf.identity().as_exp_coords()
array([0., 0., 0., 0., 0., 0.])
"""
exp_coords = self._backend.as_exp_coords(self._matrix)
if self._single:
exp_coords = exp_coords[0, ...]
return exp_coords
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def as_dual_quat(self, *, scalar_first: bool = False) -> Array:
"""Return the dual quaternion representation of the transform.
Unit dual quaternions encode orientation in a real unit quaternion
and translation in a dual quaternion. There is a double cover, i.e.,
the unit dual quaternions q and -q represent the same transform.
Parameters
----------
scalar_first : bool, optional
Whether the scalar component goes first or last in the two
individual quaternions that represent the real and the dual part.
Default is False, i.e. the scalar-last order is used.
Returns
-------
dual_quat : numpy.ndarray, shape (..., 8)
Unit dual quaternion vectors with the same leading dimensions as the
transform. The real part is stored in the first four components and the
dual part in the last four components.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
Get identity dual quaternion (we use scalar-last by default):
>>> Tf.identity().as_dual_quat()
array([0., 0., 0., 1., 0., 0., 0., 0.])
When we want to use the scalar-first convention, we use the argument:
>>> Tf.identity().as_dual_quat(scalar_first=True)
array([1., 0., 0., 0., 0., 0., 0., 0.])
"""
dual_quat = self._backend.as_dual_quat(self._matrix, scalar_first=scalar_first)
if self._single:
dual_quat = dual_quat[0]
return dual_quat
def __len__(self) -> int:
"""Return the length of the leading transform dimension.
A transform can store an N-dimensional array of transforms. The length is the
size of the first dimension of this array. If the transform is a single
transform, the length is not defined and an error is raised.
Returns
-------
length : int
The number of transforms in this object.
Raises
------
TypeError
If the transform is a single transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
>>> tf = Tf.identity(3)
>>> len(tf)
3
An N-dimensional array of transforms returns its first dimension size:
>>> t = np.ones((5, 2, 3, 1, 3))
>>> tf = Tf.from_translation(t)
>>> len(tf)
5
A single transform has no length:
>>> tf = Tf.from_translation([1, 0, 0])
>>> len(tf) # doctest: +IGNORE_EXCEPTION_DETAIL
Traceback (most recent call last):
...
TypeError: Single transform has no len().
"""
# We don't use self._single here because we also want to raise an error for
# Array API backends that call len() on single RigidTransform objects.
if self.single:
raise TypeError("Single transform has no len")
return self._matrix.shape[0]
@xp_capabilities(
jax_jit=False,
skip_backends=[("dask.array", "cannot handle zero-length rigid transforms")],
)
def __getitem__(
self, indexer: int | slice | EllipsisType | None | ArrayLike
) -> RigidTransform:
"""Extract transform(s) at given index(es) from this object.
Creates a new `RigidTransform` instance containing a subset of
transforms stored in this object.
Parameters
----------
indexer : int or slice or array_like
Specifies which transform(s) to extract. A single indexer must be
specified, i.e. as if indexing a 1 dimensional array or list.
Returns
-------
transform : `RigidTransform` instance
Contains
- a single transform, if `indexer` is a single index
- a stack of transform(s), if `indexer` is a slice, or an index
array.
Raises
------
TypeError
If the transform is a single transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> t = [[0, 0, 0], [1, 0, 0], [2, 0, 0]] # 3 translations
>>> tf = Tf.from_translation(t)
A single index returns a single transform:
>>> tf[0].as_matrix()
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
A slice returns a stack of transforms:
>>> tf[1:3].translation
array([[1., 0., 0.],
[2., 0., 0.]])
An index array returns a stack of transforms:
>>> tf[[0, 2]].translation
array([[0., 0., 0.],
[2., 0., 0.]])
"""
if self.single:
raise TypeError("Single transform is not subscriptable.")
is_array = isinstance(indexer, type(self._matrix))
xp = self._xp
# Masking is only specified in the Array API when the array is the sole index
# This special case handling is necessary to support boolean indexing and
# integer array indexing with take (see
# https://github.com/data-apis/array-api/pull/900#issuecomment-2674432480)
# Ideally we would converge to [indexer, ...] indexing, but this is not
# supported for now.
if is_array and indexer.dtype == xp.bool:
return RigidTransform(self._matrix[indexer], normalize=False)
if is_array and xp.isdtype(indexer.dtype, "integral"):
if self._matrix.shape[0] == 0:
raise IndexError("cannot take from an empty array")
return RigidTransform(
xp.take(self._matrix, indexer, axis=0), normalize=False
)
return RigidTransform(self._matrix[indexer, ...], normalize=False)
@xp_capabilities(
jax_jit=False,
skip_backends=[("dask.array", "cannot handle zero-length rigid transforms")],
)
def __setitem__(
self,
indexer: int | slice | EllipsisType | None | ArrayLike,
value: RigidTransform,
):
"""Set transform(s) at given index(es) in this object.
Parameters
----------
indexer : int or slice or array_like
Specifies which transform(s) to replace. A single indexer must be
specified, i.e. as if indexing a 1 dimensional array or list.
value : `RigidTransform` instance
The transform(s) to set.
Raises
------
TypeError
If the transform is a single transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> t = [[0, 0, 0], [1, 0, 0], [2, 0, 0]] # 3 translations
>>> tf = Tf.from_translation(t)
Set a single transform:
>>> tf[0] = Tf.from_translation([9, 9, 9])
>>> tf.translation
array([[9., 9., 9.],
[1., 0., 0.],
[2., 0., 0.]])
"""
if self.single:
raise TypeError("Single transform is not subscriptable.")
if not isinstance(value, RigidTransform):
raise TypeError("value must be a RigidTransform object")
self._matrix = self._backend.setitem(self._matrix, indexer, value.as_matrix())
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def __mul__(self, other: RigidTransform) -> RigidTransform | NotImplementedType:
"""Compose this transform with the other.
If ``p`` and ``q`` are two transforms, then the composition of '``q``
followed by ``p``' is equivalent to ``p * q``. In terms of
transformation matrices, the composition can be expressed as
``p.as_matrix() @ q.as_matrix()``.
In terms of translations and rotations, the composition when applied to
a vector ``v`` is equivalent to
``p.translation + p.rotation.apply(q.translation)
+ (p.rotation * q.rotation).apply(v)``.
This function supports composition of multiple transforms at a time using
broadcasting rules. The resulting shape for two `RigidTransform` instances
``p`` and ``q`` is `np.broadcast_shapes(p.shape, q.shape)`.
Parameters
----------
other : `RigidTransform` instance
Transform(s) to be composed with this one. The shapes must be
broadcastable.
Returns
-------
`RigidTransform` instance
The composed transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Compose two transforms:
>>> tf1 = Tf.from_translation([1, 0, 0])
>>> tf2 = Tf.from_translation([0, 1, 0])
>>> tf = tf1 * tf2
>>> tf.translation
array([1., 1., 0.])
>>> tf.single
True
When applied to a vector, the composition of two transforms is applied
in right-to-left order.
>>> t1, r1 = [1, 2, 3], R.from_euler('z', 60, degrees=True)
>>> t2, r2 = [0, 1, 0], R.from_euler('x', 30, degrees=True)
>>> tf1 = Tf.from_components(t1, r1)
>>> tf2 = Tf.from_components(t2, r2)
>>> tf = tf1 * tf2
>>> tf.apply([1, 0, 0])
array([0.6339746, 3.3660254, 3. ])
>>> tf1.apply(tf2.apply([1, 0, 0]))
array([0.6339746, 3.3660254, 3. ])
When at least one of the transforms is not single, the result is a stack
of transforms.
>>> tf1 = Tf.from_translation([1, 0, 0])
>>> tf2 = Tf.from_translation([[0, 2, 0], [0, 0, 3]])
>>> tf = tf1 * tf2
>>> tf.translation
array([[1., 2., 0.],
[1., 0., 3.]])
>>> tf.single
False
>>> len(tf)
2
Broadcasting rules apply when composing multiple transforms at a time.
>>> tf1 = Tf.from_translation(np.ones((5, 1, 3))) # Shape (5, 1, 3)
>>> tf2 = Tf.from_translation(np.ones((1, 4, 3))) # Shape (1, 4, 3)
>>> tf = tf1 * tf2 # Shape (5, 4, 3)
>>> tf.translation.shape
(5, 4, 3)
"""
if not isinstance(other, RigidTransform):
# If other is not a RigidTransform, we return NotImplemented to allow other
# types to implement __rmul__
return NotImplemented
if not broadcastable(self._matrix.shape, other._matrix.shape):
raise ValueError(
f"Cannot broadcast {self._matrix.shape[:-2]} transforms in "
f"first to {other._matrix.shape[:-2]} transforms in second object."
)
cython_compatible = self._matrix.ndim < 4 and other._matrix.ndim < 4
backend = select_backend(self._xp, cython_compatible=cython_compatible)
matrix = backend.compose_transforms(self._matrix, other._matrix)
# Only necessary for cython. Array API broadcasting handles this by default
if self._single and other._single:
matrix = matrix[0, ...]
return RigidTransform(matrix, normalize=True, copy=False)
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def __pow__(self, n: float) -> RigidTransform:
"""Compose this transform with itself `n` times.
A rigid transform `p` when raised to non-integer powers can be thought
of as finding a fraction of the transformation. For example, a power of
0.5 finds a "halfway" transform from the identity to `p`.
This is implemented by applying screw linear interpolation (ScLERP)
between `p` and the identity transform, where the angle of the rotation
component is scaled by `n`, and the translation is proportionally
adjusted along the screw axis.
``q = p ** n`` can also be expressed as
``q = RigidTransform.from_exp_coords(p.as_exp_coords() * n)``.
If `n` is negative, then the transform is inverted before the power
is applied. In other words, ``p ** -abs(n) == p.inv() ** abs(n)``.
Parameters
----------
n : float
The number of times to compose the transform with itself.
Returns
-------
`RigidTransform` instance
If the input Rotation `p` contains `N` multiple rotations, then
the output will contain `N` rotations where the `i` th rotation
is equal to ``p[i] ** n``.
Notes
-----
There are three notable cases: if ``n == 1`` then a copy of the original
transform is returned, if ``n == 0`` then the identity transform is
returned, and if ``n == -1`` then the inverse transform is returned.
Note that fractional powers ``n`` which effectively take a root of
rotation, do so using the shortest path smallest representation of that
angle (the principal root). This means that powers of ``n`` and ``1/n``
are not necessarily inverses of each other. For example, a 0.5 power of
a +240 degree rotation will be calculated as the 0.5 power of a -120
degree rotation, with the result being a rotation of -60 rather than
+120 degrees.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> import numpy as np
A power of 2 returns the transform composed with itself:
>>> tf = Tf.from_translation([1, 2, 3])
>>> (tf ** 2).translation
array([2., 4., 6.])
>>> (tf ** 2).as_matrix()
array([[1., 0., 0., 2.],
[0., 1., 0., 4.],
[0., 0., 1., 6.],
[0., 0., 0., 1.]])
A negative power returns the inverse of the transform raised to the
absolute value of `n`:
>>> (tf ** -2).translation
array([-2., -4., -6.])
>>> np.allclose((tf ** -2).as_matrix(), (tf.inv() ** 2).as_matrix(),
... atol=1e-12)
True
A power of 0 returns the identity transform:
>>> (tf ** 0).as_matrix()
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
A power of 1 returns a copy of the original transform:
>>> (tf ** 1).as_matrix()
array([[1., 0., 0., 1.],
[0., 1., 0., 2.],
[0., 0., 1., 3.],
[0., 0., 0., 1.]])
A fractional power returns a transform with a scaled rotation and
translated along the screw axis. Here we take the square root of the
transform, which when squared recovers the original transform:
>>> tf_half = (tf ** 0.5)
>>> tf_half.translation
array([0.5, 1., 1.5])
>>> (tf_half ** 2).as_matrix()
array([[1., 0., 0., 1.],
[0., 1., 0., 2.],
[0., 0., 1., 3.],
[0., 0., 0., 1.]])
"""
matrix = self._backend.pow(self._matrix, n)
if self._single:
matrix = matrix[0, ...]
return RigidTransform._from_raw_matrix(matrix, self._xp, self._backend)
@xp_capabilities(
skip_backends=[("dask.array", "missing linalg.cross/det functions")]
)
def inv(self) -> RigidTransform:
"""Invert this transform.
Composition of a transform with its inverse results in an identity
transform.
A rigid transform is a composition of a rotation and a translation,
where the rotation is applied first, followed by the translation. So the
inverse transform is equivalent to the inverse translation followed by
the inverse rotation.
Returns
-------
`RigidTransform` instance
The inverse of this transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
A transform composed with its inverse results in an identity transform:
>>> rng = np.random.default_rng(seed=123)
>>> t = rng.random(3)
>>> r = R.random(rng=rng)
>>> tf = Tf.from_components(t, r)
>>> tf.as_matrix()
array([[-0.45431291, 0.67276178, -0.58394466, 0.68235186],
[-0.23272031, 0.54310598, 0.80676958, 0.05382102],
[ 0.85990758, 0.50242162, -0.09017473, 0.22035987],
[ 0. , 0. , 0. , 1. ]])
>>> (tf.inv() * tf).as_matrix()
array([[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]])
The inverse rigid transform is the same as the inverse translation
followed by the inverse rotation:
>>> t, r = tf.as_components()
>>> r_inv = r.inv() # inverse rotation
>>> t_inv = -t # inverse translation
>>> tf_r_inv = Tf.from_rotation(r_inv)
>>> tf_t_inv = Tf.from_translation(t_inv)
>>> np.allclose((tf_r_inv * tf_t_inv).as_matrix(),
... tf.inv().as_matrix(),
... atol=1e-12)
True
>>> (tf_r_inv * tf_t_inv * tf).as_matrix()
array([[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]])
"""
matrix = self._backend.inv(self._matrix)
if self._single:
matrix = matrix[0, ...]
return RigidTransform._from_raw_matrix(matrix, self._xp, self._backend)
@xp_capabilities(
skip_backends=[
("dask.array", "missing linalg.cross/det functions"),
("cupy", "missing .mT attribute in cupy<14.*"),
]
)
def apply(self, vector: ArrayLike, inverse: bool = False) -> Array:
"""Apply the transform to a vector.
If the original frame transforms to the final frame by this transform,
then its application to a vector can be seen in two ways:
- As a projection of vector components expressed in the final frame
to the original frame.
- As the physical transformation of a vector being glued to the
original frame as it transforms. In this case the vector
components are expressed in the original frame before and after
the transformation.
In terms of rotation matrices and translation vectors, this application
is the same as
``self.translation + self.rotation.as_matrix() @ vector``.
Parameters
----------
vector : array_like, shape (..., 3)
Vector(s) to be transformed. Each vector[..., :] represents a 3D
vector.
inverse : bool, optional
If True, the inverse of the transform is applied to the vector.
Returns
-------
transformed_vector : numpy.ndarray, shape (..., 3)
The transformed vector(s) with shape determined by broadcasting
the transform and vector shapes together.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
Apply a single transform to a vector. Here the transform is just a
translation, so the result is the vector added to the translation
vector.
>>> t = np.array([1, 2, 3])
>>> tf = Tf.from_translation(t)
>>> t + np.array([1, 0, 0])
array([2, 2, 3])
>>> tf.apply([1, 0, 0])
array([2., 2., 3.])
Apply a single transform to a stack of vectors:
>>> tf.apply([[1, 0, 0], [0, 1, 0]])
array([[2., 2., 3.],
[1., 3., 3.]])
Apply the inverse of a transform to a vector, so the result is the
negative of the translation vector added to the vector.
>>> -t + np.array([1, 0, 0])
array([0, -2, -3])
>>> tf.apply([1, 0, 0], inverse=True)
array([0., -2., -3.])
Broadcasting is supported when applying multiple transforms to an N-dimensional
array of vectors.
>>> tf = Tf.from_translation(np.ones((4, 5, 1, 3)))
>>> vectors = np.zeros((2, 3))
>>> tf.apply(vectors).shape
(4, 5, 2, 3)
For transforms which are not just pure translations, applying it to a
vector is the same as applying the rotation component to the vector and
then adding the translation component.
>>> r = R.from_euler('z', 60, degrees=True)
>>> tf = Tf.from_components(t, r)
>>> t + r.apply([1, 0, 0])
array([1.5, 2.8660254, 3. ])
>>> tf.apply([1, 0, 0])
array([1.5, 2.8660254, 3. ])
When applying the inverse of a transform, the result is the negative of
the translation vector added to the vector, and then rotated by the
inverse rotation.
>>> r.inv().apply(-t + np.array([1, 0, 0]))
array([-1.73205081, -1. , -3. ])
>>> tf.apply([1, 0, 0], inverse=True)
array([-1.73205081, -1. , -3. ])
"""
# We do not use the cached xp here to catch cases where matrix and vector have
# different xp instances.
xp = array_namespace(self._matrix, vector)
vector = xp.asarray(
vector, dtype=self._matrix.dtype, device=device(self._matrix)
)
cython_compatible = self._matrix.ndim < 4 and vector.ndim < 3
backend = select_backend(xp, cython_compatible=cython_compatible)
result = backend.apply(self._matrix, vector, inverse)
if self._single and vector.ndim == 1:
result = result[0, ...]
return result
@property
def rotation(self) -> Rotation:
"""Return the rotation component of the transform.
A transform is a composition of a rotation and a translation, such that
when applied to a vector, the vector is first rotated and then
translated. This property returns the rotation part of the transform.
Returns
-------
rotation : `Rotation` instance
A single rotation or a stack of rotations.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
The rotation component is extracted from the transform:
>>> t = np.array([1, 0, 0])
>>> r = R.random(3)
>>> tf = Tf.from_components(t, r)
>>> np.allclose(tf.rotation.as_matrix(), r.as_matrix())
True
"""
if self._single:
return Rotation.from_matrix(self._matrix[0, :3, :3])
return Rotation.from_matrix(self._matrix[..., :3, :3])
@property
def translation(self) -> Array:
"""Return the translation component of the transform.
A transform is a composition of a rotation and a translation, such that
when applied to a vector, the vector is first rotated and then
translated. This property returns the translation part of the transform.
Returns
-------
translation : numpy.ndarray, shape (..., 3)
Translation vectors with the same leading dimensions as the transform.
Examples
--------
>>> from scipy.spatial.transform import RigidTransform as Tf
>>> from scipy.spatial.transform import Rotation as R
>>> import numpy as np
The translation component is extracted from the transform:
>>> t = np.array([[1, 0, 0], [2, 0, 0], [3, 0, 0]])
>>> r = R.random()
>>> tf = Tf.from_components(t, r)
>>> np.allclose(tf.translation, t)
True
"""
if self._single:
return self._xp.asarray(self._matrix[0, :3, 3], copy=True)
return self._xp.asarray(self._matrix[..., :3, 3], copy=True)
@property
def single(self) -> bool:
"""Whether this instance represents a single transform.
Single transforms are not subscriptable, and do not have a length.
Returns
-------
single : bool
True if this instance represents a single transform, False
otherwise.
"""
return self._single or self._matrix.ndim == 2
@property
def shape(self) -> tuple[int, ...]:
"""The shape of the transform's leading dimensions."""
if self._single or self._matrix.ndim == 2:
return ()
return self._matrix.shape[:-2]
def __reduce__(self) -> tuple[Callable, tuple]:
"""Reduce the RigidTransform for pickling.
We store modules inside RigidTransforms which cannot be pickled. To circumvent
this, we pickle only the matrix and restore the cached modules from the matrix
type in `from_matrix`.
"""
matrix = self._matrix
if self._single:
matrix = matrix[0, ...]
return (self.__class__.from_matrix, (matrix,))
@xp_capabilities()
def __iter__(self) -> Iterator[RigidTransform]:
"""Iterate over transforms."""
if self._single or self._matrix.ndim == 2:
raise TypeError("Single transform is not iterable.")
# We return a generator that yields a new RigidTransform object for each
# transform in the current object. We cannot rely on the default implementation
# because jax will not raise an IndexError for out-of-bounds indices.
for i in range(self._matrix.shape[0]):
yield RigidTransform._from_raw_matrix(
self._matrix[i, ...], self._xp, self._backend
)
@staticmethod
def _from_raw_matrix(
matrix: Array, xp: ModuleType, backend: ModuleType | None = None
) -> RigidTransform:
"""Create a RigidTransform skipping all sanitization steps.
This method is is intended for internal, performant creation of RigidTransforms
with matrices that are guaranteed to be valid.
"""
tf = RigidTransform.__new__(RigidTransform)
tf._single = matrix.ndim == 2 and is_numpy(xp)
if tf._single:
matrix = xpx.atleast_nd(matrix, ndim=3, xp=xp)
tf._matrix = matrix
tf._xp = xp
if backend is None:
backend = select_backend(xp, matrix.ndim < 4)
tf._backend = backend
return tf