Fixed database typo and removed unnecessary class identifier.

This commit is contained in:
Batuhan Berk Başoğlu 2020-10-14 10:10:37 -04:00
parent 00ad49a143
commit 45fb349a7d
5098 changed files with 952558 additions and 85 deletions

View file

@ -0,0 +1,26 @@
"""
Spatial Transformations (:mod:`scipy.spatial.transform`)
========================================================
.. currentmodule:: scipy.spatial.transform
This package implements various spatial transformations. For now,
only rotations are supported.
Rotations in 3 dimensions
-------------------------
.. autosummary::
:toctree: generated/
Rotation
Slerp
RotationSpline
"""
from .rotation import Rotation, Slerp
from ._rotation_spline import RotationSpline
__all__ = ['Rotation', 'Slerp', 'RotationSpline']
from scipy._lib._testutils import PytestTester
test = PytestTester(__name__)
del PytestTester

View file

@ -0,0 +1,140 @@
import numpy as np
from scipy.constants import golden as phi
def icosahedral(cls):
g1 = tetrahedral(cls).as_quat()
a = 0.5
b = 0.5 / phi
c = phi / 2
g2 = np.array([[+a, +b, +c, 0],
[+a, +b, -c, 0],
[+a, +c, 0, +b],
[+a, +c, 0, -b],
[+a, -b, +c, 0],
[+a, -b, -c, 0],
[+a, -c, 0, +b],
[+a, -c, 0, -b],
[+a, 0, +b, +c],
[+a, 0, +b, -c],
[+a, 0, -b, +c],
[+a, 0, -b, -c],
[+b, +a, 0, +c],
[+b, +a, 0, -c],
[+b, +c, +a, 0],
[+b, +c, -a, 0],
[+b, -a, 0, +c],
[+b, -a, 0, -c],
[+b, -c, +a, 0],
[+b, -c, -a, 0],
[+b, 0, +c, +a],
[+b, 0, +c, -a],
[+b, 0, -c, +a],
[+b, 0, -c, -a],
[+c, +a, +b, 0],
[+c, +a, -b, 0],
[+c, +b, 0, +a],
[+c, +b, 0, -a],
[+c, -a, +b, 0],
[+c, -a, -b, 0],
[+c, -b, 0, +a],
[+c, -b, 0, -a],
[+c, 0, +a, +b],
[+c, 0, +a, -b],
[+c, 0, -a, +b],
[+c, 0, -a, -b],
[0, +a, +c, +b],
[0, +a, +c, -b],
[0, +a, -c, +b],
[0, +a, -c, -b],
[0, +b, +a, +c],
[0, +b, +a, -c],
[0, +b, -a, +c],
[0, +b, -a, -c],
[0, +c, +b, +a],
[0, +c, +b, -a],
[0, +c, -b, +a],
[0, +c, -b, -a]])
return cls.from_quat(np.concatenate((g1, g2)))
def octahedral(cls):
g1 = tetrahedral(cls).as_quat()
c = np.sqrt(2) / 2
g2 = np.array([[+c, 0, 0, +c],
[0, +c, 0, +c],
[0, 0, +c, +c],
[0, 0, -c, +c],
[0, -c, 0, +c],
[-c, 0, 0, +c],
[0, +c, +c, 0],
[0, -c, +c, 0],
[+c, 0, +c, 0],
[-c, 0, +c, 0],
[+c, +c, 0, 0],
[-c, +c, 0, 0]])
return cls.from_quat(np.concatenate((g1, g2)))
def tetrahedral(cls):
g1 = np.eye(4)
c = 0.5
g2 = np.array([[c, -c, -c, +c],
[c, -c, +c, +c],
[c, +c, -c, +c],
[c, +c, +c, +c],
[c, -c, -c, -c],
[c, -c, +c, -c],
[c, +c, -c, -c],
[c, +c, +c, -c]])
return cls.from_quat(np.concatenate((g1, g2)))
def dicyclic(cls, n, axis=2):
g1 = cyclic(cls, n, axis).as_rotvec()
thetas = np.linspace(0, np.pi, n, endpoint=False)
rv = np.pi * np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
g2 = np.roll(rv, axis, axis=1)
return cls.from_rotvec(np.concatenate((g1, g2)))
def cyclic(cls, n, axis=2):
thetas = np.linspace(0, 2 * np.pi, n, endpoint=False)
rv = np.vstack([thetas, np.zeros(n), np.zeros(n)]).T
return cls.from_rotvec(np.roll(rv, axis, axis=1))
def create_group(cls, group, axis='Z'):
if not isinstance(group, str):
raise ValueError("`group` argument must be a string")
permitted_axes = ['x', 'y', 'z', 'X', 'Y', 'Z']
if axis not in permitted_axes:
raise ValueError("`axis` must be one of " + ", ".join(permitted_axes))
if group in ['I', 'O', 'T']:
symbol = group
order = 1
elif group[:1] in ['C', 'D'] and group[1:].isdigit():
symbol = group[:1]
order = int(group[1:])
else:
raise ValueError("`group` must be one of 'I', 'O', 'T', 'Dn', 'Cn'")
if order < 1:
raise ValueError("Group order must be positive")
axis = 'xyz'.index(axis.lower())
if symbol == 'I':
return icosahedral(cls)
elif symbol == 'O':
return octahedral(cls)
elif symbol == 'T':
return tetrahedral(cls)
elif symbol == 'D':
return dicyclic(cls, order, axis=axis)
elif symbol == 'C':
return cyclic(cls, order, axis=axis)
else:
assert False

View file

@ -0,0 +1,456 @@
import numpy as np
from scipy.linalg import solve_banded
from .rotation import Rotation
def _create_skew_matrix(x):
"""Create skew-symmetric matrices corresponding to vectors.
Parameters
----------
x : ndarray, shape (n, 3)
Set of vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
result = np.zeros((len(x), 3, 3))
result[:, 0, 1] = -x[:, 2]
result[:, 0, 2] = x[:, 1]
result[:, 1, 0] = x[:, 2]
result[:, 1, 2] = -x[:, 0]
result[:, 2, 0] = -x[:, 1]
result[:, 2, 1] = x[:, 0]
return result
def _matrix_vector_product_of_stacks(A, b):
"""Compute the product of stack of matrices and vectors."""
return np.einsum("ijk,ik->ij", A, b)
def _angular_rate_to_rotvec_dot_matrix(rotvecs):
"""Compute matrices to transform angular rates to rot. vector derivatives.
The matrices depend on the current attitude represented as a rotation
vector.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
k = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k[mask] = (1 - 0.5 * nm / np.tan(0.5 * nm)) / nm**2
mask = ~mask
nm = norm[mask]
k[mask] = 1/12 + 1/720 * nm**2
skew = _create_skew_matrix(rotvecs)
result = np.empty((len(rotvecs), 3, 3))
result[:] = np.identity(3)
result[:] += 0.5 * skew
result[:] += k[:, None, None] * np.matmul(skew, skew)
return result
def _rotvec_dot_to_angular_rate_matrix(rotvecs):
"""Compute matrices to transform rot. vector derivatives to angular rates.
The matrices depend on the current attitude represented as a rotation
vector.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
k1 = np.empty_like(norm)
k2 = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k1[mask] = (1 - np.cos(nm)) / nm ** 2
k2[mask] = (nm - np.sin(nm)) / nm ** 3
mask = ~mask
nm = norm[mask]
k1[mask] = 0.5 - nm ** 2 / 24
k2[mask] = 1 / 6 - nm ** 2 / 120
skew = _create_skew_matrix(rotvecs)
result = np.empty((len(rotvecs), 3, 3))
result[:] = np.identity(3)
result[:] -= k1[:, None, None] * skew
result[:] += k2[:, None, None] * np.matmul(skew, skew)
return result
def _angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot):
"""Compute the non-linear term in angular acceleration.
The angular acceleration contains a quadratic term with respect to
the derivative of the rotation vector. This function computes that.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot: ndarray, shape (n, 3)
Set of rotation vector derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
dp = np.sum(rotvecs * rotvecs_dot, axis=1)
cp = np.cross(rotvecs, rotvecs_dot)
ccp = np.cross(rotvecs, cp)
dccp = np.cross(rotvecs_dot, cp)
k1 = np.empty_like(norm)
k2 = np.empty_like(norm)
k3 = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k1[mask] = (-nm * np.sin(nm) - 2 * (np.cos(nm) - 1)) / nm ** 4
k2[mask] = (-2 * nm + 3 * np.sin(nm) - nm * np.cos(nm)) / nm ** 5
k3[mask] = (nm - np.sin(nm)) / nm ** 3
mask = ~mask
nm = norm[mask]
k1[mask] = 1/12 - nm ** 2 / 180
k2[mask] = -1/60 + nm ** 2 / 12604
k3[mask] = 1/6 - nm ** 2 / 120
dp = dp[:, None]
k1 = k1[:, None]
k2 = k2[:, None]
k3 = k3[:, None]
return dp * (k1 * cp + k2 * ccp) + k3 * dccp
def _compute_angular_rate(rotvecs, rotvecs_dot):
"""Compute angular rates given rotation vectors and its derivatives.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot : ndarray, shape (n, 3)
Set of rotation vector derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
return _matrix_vector_product_of_stacks(
_rotvec_dot_to_angular_rate_matrix(rotvecs), rotvecs_dot)
def _compute_angular_acceleration(rotvecs, rotvecs_dot, rotvecs_dot_dot):
"""Compute angular acceleration given rotation vector and its derivatives.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot : ndarray, shape (n, 3)
Set of rotation vector derivatives.
rotvecs_dot_dot : ndarray, shape (n, 3)
Set of rotation vector second derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
return (_compute_angular_rate(rotvecs, rotvecs_dot_dot) +
_angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot))
def _create_block_3_diagonal_matrix(A, B, d):
"""Create a 3-diagonal block matrix as banded.
The matrix has the following structure:
DB...
ADB..
.ADB.
..ADB
...AD
The blocks A, B and D are 3-by-3 matrices. The D matrices has the form
d * I.
Parameters
----------
A : ndarray, shape (n, 3, 3)
Stack of A blocks.
B : ndarray, shape (n, 3, 3)
Stack of B blocks.
d : ndarray, shape (n + 1,)
Values for diagonal blocks.
Returns
-------
ndarray, shape (11, 3 * (n + 1))
Matrix in the banded form as used by `scipy.linalg.solve_banded`.
"""
ind = np.arange(3)
ind_blocks = np.arange(len(A))
A_i = np.empty_like(A, dtype=int)
A_i[:] = ind[:, None]
A_i += 3 * (1 + ind_blocks[:, None, None])
A_j = np.empty_like(A, dtype=int)
A_j[:] = ind
A_j += 3 * ind_blocks[:, None, None]
B_i = np.empty_like(B, dtype=int)
B_i[:] = ind[:, None]
B_i += 3 * ind_blocks[:, None, None]
B_j = np.empty_like(B, dtype=int)
B_j[:] = ind
B_j += 3 * (1 + ind_blocks[:, None, None])
diag_i = diag_j = np.arange(3 * len(d))
i = np.hstack((A_i.ravel(), B_i.ravel(), diag_i))
j = np.hstack((A_j.ravel(), B_j.ravel(), diag_j))
values = np.hstack((A.ravel(), B.ravel(), np.repeat(d, 3)))
u = 5
l = 5
result = np.zeros((u + l + 1, 3 * len(d)))
result[u + i - j, j] = values
return result
class RotationSpline(object):
"""Interpolate rotations with continuous angular rate and acceleration.
The rotation vectors between each consecutive orientation are cubic
functions of time and it is guaranteed that angular rate and acceleration
are continuous. Such interpolation are analogous to cubic spline
interpolation.
Refer to [1]_ for math and implementation details.
Parameters
----------
times : array_like, shape (N,)
Times of the known rotations. At least 2 times must be specified.
rotations : `Rotation` instance
Rotations to perform the interpolation between. Must contain N
rotations.
Methods
-------
__call__
References
----------
.. [1] `Smooth Attitude Interpolation
<https://github.com/scipy/scipy/files/2932755/attitude_interpolation.pdf>`_
Examples
--------
>>> from scipy.spatial.transform import Rotation, RotationSpline
Define the sequence of times and rotations from the Euler angles:
>>> times = [0, 10, 20, 40]
>>> angles = [[-10, 20, 30], [0, 15, 40], [-30, 45, 30], [20, 45, 90]]
>>> rotations = Rotation.from_euler('XYZ', angles, degrees=True)
Create the interpolator object:
>>> spline = RotationSpline(times, rotations)
Interpolate the Euler angles, angular rate and acceleration:
>>> angular_rate = np.rad2deg(spline(times, 1))
>>> angular_acceleration = np.rad2deg(spline(times, 2))
>>> times_plot = np.linspace(times[0], times[-1], 100)
>>> angles_plot = spline(times_plot).as_euler('XYZ', degrees=True)
>>> angular_rate_plot = np.rad2deg(spline(times_plot, 1))
>>> angular_acceleration_plot = np.rad2deg(spline(times_plot, 2))
On this plot you see that Euler angles are continuous and smooth:
>>> import matplotlib.pyplot as plt
>>> plt.plot(times_plot, angles_plot)
>>> plt.plot(times, angles, 'x')
>>> plt.title("Euler angles")
>>> plt.show()
The angular rate is also smooth:
>>> plt.plot(times_plot, angular_rate_plot)
>>> plt.plot(times, angular_rate, 'x')
>>> plt.title("Angular rate")
>>> plt.show()
The angular acceleration is continuous, but not smooth. Also note that
the angular acceleration is not a piecewise-linear function, because
it is different from the second derivative of the rotation vector (which
is a piecewise-linear function as in the cubic spline).
>>> plt.plot(times_plot, angular_acceleration_plot)
>>> plt.plot(times, angular_acceleration, 'x')
>>> plt.title("Angular acceleration")
>>> plt.show()
"""
# Parameters for the solver for angular rate.
MAX_ITER = 10
TOL = 1e-9
def _solve_for_angular_rates(self, dt, angular_rates, rotvecs):
angular_rate_first = angular_rates[0].copy()
A = _angular_rate_to_rotvec_dot_matrix(rotvecs)
A_inv = _rotvec_dot_to_angular_rate_matrix(rotvecs)
M = _create_block_3_diagonal_matrix(
2 * A_inv[1:-1] / dt[1:-1, None, None],
2 * A[1:-1] / dt[1:-1, None, None],
4 * (1 / dt[:-1] + 1 / dt[1:]))
b0 = 6 * (rotvecs[:-1] * dt[:-1, None] ** -2 +
rotvecs[1:] * dt[1:, None] ** -2)
b0[0] -= 2 / dt[0] * A_inv[0].dot(angular_rate_first)
b0[-1] -= 2 / dt[-1] * A[-1].dot(angular_rates[-1])
for iteration in range(self.MAX_ITER):
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
delta_beta = _angular_acceleration_nonlinear_term(
rotvecs[:-1], rotvecs_dot[:-1])
b = b0 - delta_beta
angular_rates_new = solve_banded((5, 5), M, b.ravel())
angular_rates_new = angular_rates_new.reshape((-1, 3))
delta = np.abs(angular_rates_new - angular_rates[:-1])
angular_rates[:-1] = angular_rates_new
if np.all(delta < self.TOL * (1 + np.abs(angular_rates_new))):
break
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
angular_rates = np.vstack((angular_rate_first, angular_rates[:-1]))
return angular_rates, rotvecs_dot
def __init__(self, times, rotations):
from scipy.interpolate import PPoly
if len(rotations) == 1:
raise ValueError("`rotations` must contain at least 2 rotations.")
times = np.asarray(times, dtype=float)
if times.ndim != 1:
raise ValueError("`times` must be 1-dimensional.")
if len(times) != len(rotations):
raise ValueError("Expected number of rotations to be equal to "
"number of timestamps given, got {} rotations "
"and {} timestamps."
.format(len(rotations), len(times)))
dt = np.diff(times)
if np.any(dt <= 0):
raise ValueError("Values in `times` must be in a strictly "
"increasing order.")
rotvecs = (rotations[:-1].inv() * rotations[1:]).as_rotvec()
angular_rates = rotvecs / dt[:, None]
if len(rotations) == 2:
rotvecs_dot = angular_rates
else:
angular_rates, rotvecs_dot = self._solve_for_angular_rates(
dt, angular_rates, rotvecs)
dt = dt[:, None]
coeff = np.empty((4, len(times) - 1, 3))
coeff[0] = (-2 * rotvecs + dt * angular_rates
+ dt * rotvecs_dot) / dt ** 3
coeff[1] = (3 * rotvecs - 2 * dt * angular_rates
- dt * rotvecs_dot) / dt ** 2
coeff[2] = angular_rates
coeff[3] = 0
self.times = times
self.rotations = rotations
self.interpolator = PPoly(coeff, times)
def __call__(self, times, order=0):
"""Compute interpolated values.
Parameters
----------
times : float or array_like
Times of interest.
order : {0, 1, 2}, optional
Order of differentiation:
* 0 (default) : return Rotation
* 1 : return the angular rate in rad/sec
* 2 : return the angular acceleration in rad/sec/sec
Returns
-------
Interpolated Rotation, angular rate or acceleration.
"""
if order not in [0, 1, 2]:
raise ValueError("`order` must be 0, 1 or 2.")
times = np.asarray(times, dtype=float)
if times.ndim > 1:
raise ValueError("`times` must be at most 1-dimensional.")
singe_time = times.ndim == 0
times = np.atleast_1d(times)
rotvecs = self.interpolator(times)
if order == 0:
index = np.searchsorted(self.times, times, side='right')
index -= 1
index[index < 0] = 0
n_segments = len(self.times) - 1
index[index > n_segments - 1] = n_segments - 1
result = self.rotations[index] * Rotation.from_rotvec(rotvecs)
elif order == 1:
rotvecs_dot = self.interpolator(times, 1)
result = _compute_angular_rate(rotvecs, rotvecs_dot)
elif order == 2:
rotvecs_dot = self.interpolator(times, 1)
rotvecs_dot_dot = self.interpolator(times, 2)
result = _compute_angular_acceleration(rotvecs, rotvecs_dot,
rotvecs_dot_dot)
else:
assert False
if singe_time:
result = result[0]
return result

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,9 @@
def configuration(parent_package='', top_path=None):
from numpy.distutils.misc_util import Configuration
config = Configuration('transform', parent_package, top_path)
config.add_data_dir('tests')
return config

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,169 @@
import pytest
import numpy as np
from numpy.testing import assert_array_almost_equal
from scipy.spatial.transform import Rotation
from scipy.optimize import linear_sum_assignment
from scipy.spatial.distance import cdist
from scipy.constants import golden as phi
from scipy.spatial import cKDTree
TOL = 1E-12
NS = range(1, 13)
NAMES = ["I", "O", "T"] + ["C%d" % n for n in NS] + ["D%d" % n for n in NS]
SIZES = [60, 24, 12] + list(NS) + [2 * n for n in NS]
def _calculate_rmsd(P, Q):
"""Calculates the root-mean-square distance between the points of P and Q.
The distance is taken as the minimum over all possible matchings. It is
zero if P and Q are identical and non-zero if not.
"""
distance_matrix = cdist(P, Q, metric='sqeuclidean')
matching = linear_sum_assignment(distance_matrix)
return np.sqrt(distance_matrix[matching].sum())
def _generate_pyramid(n, axis):
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
P = np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
P = np.concatenate((P, [[1, 0, 0]]))
return np.roll(P, axis, axis=1)
def _generate_prism(n, axis):
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
bottom = np.vstack([-np.ones(n), np.cos(thetas), np.sin(thetas)]).T
top = np.vstack([+np.ones(n), np.cos(thetas), np.sin(thetas)]).T
P = np.concatenate((bottom, top))
return np.roll(P, axis, axis=1)
def _generate_icosahedron():
x = np.array([[0, -1, -phi],
[0, -1, +phi],
[0, +1, -phi],
[0, +1, +phi]])
return np.concatenate([np.roll(x, i, axis=1) for i in range(3)])
def _generate_octahedron():
return np.array([[-1, 0, 0], [+1, 0, 0], [0, -1, 0],
[0, +1, 0], [0, 0, -1], [0, 0, +1]])
def _generate_tetrahedron():
return np.array([[1, 1, 1], [1, -1, -1], [-1, 1, -1], [-1, -1, 1]])
@pytest.mark.parametrize("name", [-1, None, True, np.array(['C3'])])
def test_group_type(name):
with pytest.raises(ValueError,
match="must be a string"):
Rotation.create_group(name)
@pytest.mark.parametrize("name", ["Q", " ", "CA", "C ", "DA", "D ", "I2", ""])
def test_group_name(name):
with pytest.raises(ValueError,
match="must be one of 'I', 'O', 'T', 'Dn', 'Cn'"):
Rotation.create_group(name)
@pytest.mark.parametrize("name", ["C0", "D0"])
def test_group_order_positive(name):
with pytest.raises(ValueError,
match="Group order must be positive"):
Rotation.create_group(name)
@pytest.mark.parametrize("axis", ['A', 'b', 0, 1, 2, 4, False, None])
def test_axis_valid(axis):
with pytest.raises(ValueError,
match="`axis` must be one of"):
Rotation.create_group("C1", axis)
def test_icosahedral():
"""The icosahedral group fixes the rotations of an icosahedron. Here we
test that the icosahedron is invariant after application of the elements
of the rotation group."""
P = _generate_icosahedron()
for g in Rotation.create_group("I"):
g = Rotation.from_quat(g.as_quat())
assert _calculate_rmsd(P, g.apply(P)) < TOL
def test_octahedral():
"""Test that the octahedral group correctly fixes the rotations of an
octahedron."""
P = _generate_octahedron()
for g in Rotation.create_group("O"):
assert _calculate_rmsd(P, g.apply(P)) < TOL
def test_tetrahedral():
"""Test that the tetrahedral group correctly fixes the rotations of a
tetrahedron."""
P = _generate_tetrahedron()
for g in Rotation.create_group("T"):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("n", NS)
@pytest.mark.parametrize("axis", 'XYZ')
def test_dicyclic(n, axis):
"""Test that the dicyclic group correctly fixes the rotations of a
prism."""
P = _generate_prism(n, axis='XYZ'.index(axis))
for g in Rotation.create_group("D%d" % n, axis=axis):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("n", NS)
@pytest.mark.parametrize("axis", 'XYZ')
def test_cyclic(n, axis):
"""Test that the cyclic group correctly fixes the rotations of a
pyramid."""
P = _generate_pyramid(n, axis='XYZ'.index(axis))
for g in Rotation.create_group("C%d" % n, axis=axis):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_sizes(name, size):
assert len(Rotation.create_group(name)) == size
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_no_duplicates(name, size):
g = Rotation.create_group(name)
kdtree = cKDTree(g.as_quat())
assert len(kdtree.query_pairs(1E-3)) == 0
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_symmetry(name, size):
g = Rotation.create_group(name)
q = np.concatenate((-g.as_quat(), g.as_quat()))
distance = np.sort(cdist(q, q))
deltas = np.max(distance, axis=0) - np.min(distance, axis=0)
assert (deltas < TOL).all()
@pytest.mark.parametrize("name", NAMES)
def test_reduction(name):
"""Test that the elements of the rotation group are correctly
mapped onto the identity rotation."""
g = Rotation.create_group(name)
f = g.reduce(g)
assert_array_almost_equal(f.magnitude(), np.zeros(len(g)))
@pytest.mark.parametrize("name", NAMES)
def test_single_reduction(name):
g = Rotation.create_group(name)
f = g[-1].reduce(g)
assert_array_almost_equal(f.magnitude(), 0)
assert f.as_quat().shape == (4,)

View file

@ -0,0 +1,161 @@
from itertools import product
import numpy as np
from numpy.testing import assert_allclose
from pytest import raises
from scipy.spatial.transform import Rotation, RotationSpline
from scipy.spatial.transform._rotation_spline import (
_angular_rate_to_rotvec_dot_matrix,
_rotvec_dot_to_angular_rate_matrix,
_matrix_vector_product_of_stacks,
_angular_acceleration_nonlinear_term,
_create_block_3_diagonal_matrix)
def test_angular_rate_to_rotvec_conversions():
np.random.seed(0)
rv = np.random.randn(4, 3)
A = _angular_rate_to_rotvec_dot_matrix(rv)
A_inv = _rotvec_dot_to_angular_rate_matrix(rv)
# When the rotation vector is aligned with the angular rate, then
# the rotation vector rate and angular rate are the same.
assert_allclose(_matrix_vector_product_of_stacks(A, rv), rv)
assert_allclose(_matrix_vector_product_of_stacks(A_inv, rv), rv)
# A and A_inv must be reciprocal to each other.
I_stack = np.empty((4, 3, 3))
I_stack[:] = np.eye(3)
assert_allclose(np.matmul(A, A_inv), I_stack, atol=1e-15)
def test_angular_rate_nonlinear_term():
# The only simple test is to check that the term is zero when
# the rotation vector
np.random.seed(0)
rv = np.random.rand(4, 3)
assert_allclose(_angular_acceleration_nonlinear_term(rv, rv), 0,
atol=1e-19)
def test_create_block_3_diagonal_matrix():
np.random.seed(0)
A = np.empty((4, 3, 3))
A[:] = np.arange(1, 5)[:, None, None]
B = np.empty((4, 3, 3))
B[:] = -np.arange(1, 5)[:, None, None]
d = 10 * np.arange(10, 15)
banded = _create_block_3_diagonal_matrix(A, B, d)
# Convert the banded matrix to the full matrix.
k, l = list(zip(*product(np.arange(banded.shape[0]),
np.arange(banded.shape[1]))))
k = np.asarray(k)
l = np.asarray(l)
i = k - 5 + l
j = l
values = banded.ravel()
mask = (i >= 0) & (i < 15)
i = i[mask]
j = j[mask]
values = values[mask]
full = np.zeros((15, 15))
full[i, j] = values
zero = np.zeros((3, 3))
eye = np.eye(3)
# Create the reference full matrix in the most straightforward manner.
ref = np.block([
[d[0] * eye, B[0], zero, zero, zero],
[A[0], d[1] * eye, B[1], zero, zero],
[zero, A[1], d[2] * eye, B[2], zero],
[zero, zero, A[2], d[3] * eye, B[3]],
[zero, zero, zero, A[3], d[4] * eye],
])
assert_allclose(full, ref, atol=1e-19)
def test_spline_2_rotations():
times = [0, 10]
rotations = Rotation.from_euler('xyz', [[0, 0, 0], [10, -20, 30]],
degrees=True)
spline = RotationSpline(times, rotations)
rv = (rotations[0].inv() * rotations[1]).as_rotvec()
rate = rv / (times[1] - times[0])
times_check = np.array([-1, 5, 12])
dt = times_check - times[0]
rv_ref = rate * dt[:, None]
assert_allclose(spline(times_check).as_rotvec(), rv_ref)
assert_allclose(spline(times_check, 1), np.resize(rate, (3, 3)))
assert_allclose(spline(times_check, 2), 0, atol=1e-16)
def test_constant_attitude():
times = np.arange(10)
rotations = Rotation.from_rotvec(np.ones((10, 3)))
spline = RotationSpline(times, rotations)
times_check = np.linspace(-1, 11)
assert_allclose(spline(times_check).as_rotvec(), 1, rtol=1e-15)
assert_allclose(spline(times_check, 1), 0, atol=1e-19)
assert_allclose(spline(times_check, 2), 0, atol=1e-19)
assert_allclose(spline(5.5).as_rotvec(), 1, rtol=1e-15)
assert_allclose(spline(5.5, 1), 0, atol=1e-19)
assert_allclose(spline(5.5, 2), 0, atol=1e-19)
def test_spline_properties():
times = np.array([0, 5, 15, 27])
angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]
rotations = Rotation.from_euler('xyz', angles, degrees=True)
spline = RotationSpline(times, rotations)
assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])
h = 1e-8
rv0 = spline(times).as_rotvec()
rvm = spline(times - h).as_rotvec()
rvp = spline(times + h).as_rotvec()
assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1e-15)
r0 = spline(times, 1)
rm = spline(times - h, 1)
rp = spline(times + h, 1)
assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)
a0 = spline(times, 2)
am = spline(times - h, 2)
ap = spline(times + h, 2)
assert_allclose(a0, am, rtol=1e-7)
assert_allclose(a0, ap, rtol=1e-7)
def test_error_handling():
raises(ValueError, RotationSpline, [1.0], Rotation.random())
r = Rotation.random(10)
t = np.arange(10).reshape(5, 2)
raises(ValueError, RotationSpline, t, r)
t = np.arange(9)
raises(ValueError, RotationSpline, t, r)
t = np.arange(10)
t[5] = 0
raises(ValueError, RotationSpline, t, r)
t = np.arange(10)
s = RotationSpline(t, r)
raises(ValueError, s, 10, -1)
raises(ValueError, s, np.arange(10).reshape(5, 2))