Coverage for /home/martinb/.local/share/virtualenvs/camcops/lib/python3.6/site-packages/scipy/spatial/transform/rotation.py : 11%

Hot-keys on this page
r m x p toggle line displays
j k next/prev highlighted chunk
0 (zero) top of page
1 (one) first highlighted chunk
1import re
2import warnings
3import numpy as np
4import scipy.linalg
5from scipy._lib._util import check_random_state
6from ._rotation_groups import create_group
9_AXIS_TO_IND = {'x': 0, 'y': 1, 'z': 2}
12def _elementary_basis_vector(axis):
13 b = np.zeros(3)
14 b[_AXIS_TO_IND[axis]] = 1
15 return b
18def _compute_euler_from_matrix(matrix, seq, extrinsic=False):
19 # The algorithm assumes intrinsic frame transformations. The algorithm
20 # in the paper is formulated for rotation matrices which are transposition
21 # rotation matrices used within Rotation.
22 # Adapt the algorithm for our case by
23 # 1. Instead of transposing our representation, use the transpose of the
24 # O matrix as defined in the paper, and be careful to swap indices
25 # 2. Reversing both axis sequence and angles for extrinsic rotations
27 if extrinsic:
28 seq = seq[::-1]
30 if matrix.ndim == 2:
31 matrix = matrix[None, :, :]
32 num_rotations = matrix.shape[0]
34 # Step 0
35 # Algorithm assumes axes as column vectors, here we use 1D vectors
36 n1 = _elementary_basis_vector(seq[0])
37 n2 = _elementary_basis_vector(seq[1])
38 n3 = _elementary_basis_vector(seq[2])
40 # Step 2
41 sl = np.dot(np.cross(n1, n2), n3)
42 cl = np.dot(n1, n3)
44 # angle offset is lambda from the paper referenced in [2] from docstring of
45 # `as_euler` function
46 offset = np.arctan2(sl, cl)
47 c = np.vstack((n2, np.cross(n1, n2), n1))
49 # Step 3
50 rot = np.array([
51 [1, 0, 0],
52 [0, cl, sl],
53 [0, -sl, cl],
54 ])
55 res = np.einsum('...ij,...jk->...ik', c, matrix)
56 matrix_transformed = np.einsum('...ij,...jk->...ik', res, c.T.dot(rot))
58 # Step 4
59 angles = np.empty((num_rotations, 3))
60 # Ensure less than unit norm
61 positive_unity = matrix_transformed[:, 2, 2] > 1
62 negative_unity = matrix_transformed[:, 2, 2] < -1
63 matrix_transformed[positive_unity, 2, 2] = 1
64 matrix_transformed[negative_unity, 2, 2] = -1
65 angles[:, 1] = np.arccos(matrix_transformed[:, 2, 2])
67 # Steps 5, 6
68 eps = 1e-7
69 safe1 = (np.abs(angles[:, 1]) >= eps)
70 safe2 = (np.abs(angles[:, 1] - np.pi) >= eps)
72 # Step 4 (Completion)
73 angles[:, 1] += offset
75 # 5b
76 safe_mask = np.logical_and(safe1, safe2)
77 angles[safe_mask, 0] = np.arctan2(matrix_transformed[safe_mask, 0, 2],
78 -matrix_transformed[safe_mask, 1, 2])
79 angles[safe_mask, 2] = np.arctan2(matrix_transformed[safe_mask, 2, 0],
80 matrix_transformed[safe_mask, 2, 1])
82 if extrinsic:
83 # For extrinsic, set first angle to zero so that after reversal we
84 # ensure that third angle is zero
85 # 6a
86 angles[~safe_mask, 0] = 0
87 # 6b
88 angles[~safe1, 2] = np.arctan2(matrix_transformed[~safe1, 1, 0]
89 - matrix_transformed[~safe1, 0, 1],
90 matrix_transformed[~safe1, 0, 0]
91 + matrix_transformed[~safe1, 1, 1])
92 # 6c
93 angles[~safe2, 2] = -np.arctan2(matrix_transformed[~safe2, 1, 0]
94 + matrix_transformed[~safe2, 0, 1],
95 matrix_transformed[~safe2, 0, 0]
96 - matrix_transformed[~safe2, 1, 1])
97 else:
98 # For instrinsic, set third angle to zero
99 # 6a
100 angles[~safe_mask, 2] = 0
101 # 6b
102 angles[~safe1, 0] = np.arctan2(matrix_transformed[~safe1, 1, 0]
103 - matrix_transformed[~safe1, 0, 1],
104 matrix_transformed[~safe1, 0, 0]
105 + matrix_transformed[~safe1, 1, 1])
106 # 6c
107 angles[~safe2, 0] = np.arctan2(matrix_transformed[~safe2, 1, 0]
108 + matrix_transformed[~safe2, 0, 1],
109 matrix_transformed[~safe2, 0, 0]
110 - matrix_transformed[~safe2, 1, 1])
112 # Step 7
113 if seq[0] == seq[2]:
114 # lambda = 0, so we can only ensure angle2 -> [0, pi]
115 adjust_mask = np.logical_or(angles[:, 1] < 0, angles[:, 1] > np.pi)
116 else:
117 # lambda = + or - pi/2, so we can ensure angle2 -> [-pi/2, pi/2]
118 adjust_mask = np.logical_or(angles[:, 1] < -np.pi / 2,
119 angles[:, 1] > np.pi / 2)
121 # Dont adjust gimbal locked angle sequences
122 adjust_mask = np.logical_and(adjust_mask, safe_mask)
124 angles[adjust_mask, 0] += np.pi
125 angles[adjust_mask, 1] = 2 * offset - angles[adjust_mask, 1]
126 angles[adjust_mask, 2] -= np.pi
128 angles[angles < -np.pi] += 2 * np.pi
129 angles[angles > np.pi] -= 2 * np.pi
131 # Step 8
132 if not np.all(safe_mask):
133 warnings.warn("Gimbal lock detected. Setting third angle to zero since"
134 " it is not possible to uniquely determine all angles.")
136 # Reverse role of extrinsic and intrinsic rotations, but let third angle be
137 # zero for gimbal locked cases
138 if extrinsic:
139 angles = angles[:, ::-1]
140 return angles
143def _make_elementary_quat(axis, angles):
144 quat = np.zeros((angles.shape[0], 4))
146 quat[:, 3] = np.cos(angles / 2)
147 quat[:, _AXIS_TO_IND[axis]] = np.sin(angles / 2)
148 return quat
151def _compose_quat(p, q):
152 product = np.empty((max(p.shape[0], q.shape[0]), 4))
153 product[:, 3] = p[:, 3] * q[:, 3] - np.sum(p[:, :3] * q[:, :3], axis=1)
154 product[:, :3] = (p[:, None, 3] * q[:, :3] + q[:, None, 3] * p[:, :3] +
155 np.cross(p[:, :3], q[:, :3]))
156 return product
159def _elementary_quat_compose(seq, angles, intrinsic=False):
160 result = _make_elementary_quat(seq[0], angles[:, 0])
162 for idx, axis in enumerate(seq[1:], start=1):
163 if intrinsic:
164 result = _compose_quat(
165 result,
166 _make_elementary_quat(axis, angles[:, idx]))
167 else:
168 result = _compose_quat(
169 _make_elementary_quat(axis, angles[:, idx]),
170 result)
171 return result
174class Rotation(object):
175 """Rotation in 3 dimensions.
177 This class provides an interface to initialize from and represent rotations
178 with:
180 - Quaternions
181 - Rotation Matrices
182 - Rotation Vectors
183 - Euler Angles
185 The following operations on rotations are supported:
187 - Application on vectors
188 - Rotation Composition
189 - Rotation Inversion
190 - Rotation Indexing
192 Indexing within a rotation is supported since multiple rotation transforms
193 can be stored within a single `Rotation` instance.
195 To create `Rotation` objects use ``from_...`` methods (see examples below).
196 ``Rotation(...)`` is not supposed to be instantiated directly.
198 Methods
199 -------
200 __len__
201 from_quat
202 from_matrix
203 from_rotvec
204 from_euler
205 as_quat
206 as_matrix
207 as_rotvec
208 as_euler
209 apply
210 __mul__
211 inv
212 magnitude
213 mean
214 reduce
215 create_group
216 __getitem__
217 identity
218 random
219 align_vectors
221 See Also
222 --------
223 Slerp
225 Notes
226 -----
227 .. versionadded: 1.2.0
229 Examples
230 --------
231 >>> from scipy.spatial.transform import Rotation as R
233 A `Rotation` instance can be initialized in any of the above formats and
234 converted to any of the others. The underlying object is independent of the
235 representation used for initialization.
237 Consider a counter-clockwise rotation of 90 degrees about the z-axis. This
238 corresponds to the following quaternion (in scalar-last format):
240 >>> r = R.from_quat([0, 0, np.sin(np.pi/4), np.cos(np.pi/4)])
242 The rotation can be expressed in any of the other formats:
244 >>> r.as_matrix()
245 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
246 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
247 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
248 >>> r.as_rotvec()
249 array([0. , 0. , 1.57079633])
250 >>> r.as_euler('zyx', degrees=True)
251 array([90., 0., 0.])
253 The same rotation can be initialized using a rotation matrix:
255 >>> r = R.from_matrix([[0, -1, 0],
256 ... [1, 0, 0],
257 ... [0, 0, 1]])
259 Representation in other formats:
261 >>> r.as_quat()
262 array([0. , 0. , 0.70710678, 0.70710678])
263 >>> r.as_rotvec()
264 array([0. , 0. , 1.57079633])
265 >>> r.as_euler('zyx', degrees=True)
266 array([90., 0., 0.])
268 The rotation vector corresponding to this rotation is given by:
270 >>> r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1]))
272 Representation in other formats:
274 >>> r.as_quat()
275 array([0. , 0. , 0.70710678, 0.70710678])
276 >>> r.as_matrix()
277 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
278 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
279 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
280 >>> r.as_euler('zyx', degrees=True)
281 array([90., 0., 0.])
283 The ``from_euler`` method is quite flexible in the range of input formats
284 it supports. Here we initialize a single rotation about a single axis:
286 >>> r = R.from_euler('z', 90, degrees=True)
288 Again, the object is representation independent and can be converted to any
289 other format:
291 >>> r.as_quat()
292 array([0. , 0. , 0.70710678, 0.70710678])
293 >>> r.as_matrix()
294 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
295 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
296 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
297 >>> r.as_rotvec()
298 array([0. , 0. , 1.57079633])
300 It is also possible to initialize multiple rotations in a single instance
301 using any of the `from_...` functions. Here we initialize a stack of 3
302 rotations using the ``from_euler`` method:
304 >>> r = R.from_euler('zyx', [
305 ... [90, 0, 0],
306 ... [0, 45, 0],
307 ... [45, 60, 30]], degrees=True)
309 The other representations also now return a stack of 3 rotations. For
310 example:
312 >>> r.as_quat()
313 array([[0. , 0. , 0.70710678, 0.70710678],
314 [0. , 0.38268343, 0. , 0.92387953],
315 [0.39190384, 0.36042341, 0.43967974, 0.72331741]])
317 Applying the above rotations onto a vector:
319 >>> v = [1, 2, 3]
320 >>> r.apply(v)
321 array([[-2. , 1. , 3. ],
322 [ 2.82842712, 2. , 1.41421356],
323 [ 2.24452282, 0.78093109, 2.89002836]])
325 A `Rotation` instance can be indexed and sliced as if it were a single
326 1D array or list:
328 >>> r.as_quat()
329 array([[0. , 0. , 0.70710678, 0.70710678],
330 [0. , 0.38268343, 0. , 0.92387953],
331 [0.39190384, 0.36042341, 0.43967974, 0.72331741]])
332 >>> p = r[0]
333 >>> p.as_matrix()
334 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
335 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
336 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
337 >>> q = r[1:3]
338 >>> q.as_quat()
339 array([[0. , 0.38268343, 0. , 0.92387953],
340 [0.39190384, 0.36042341, 0.43967974, 0.72331741]])
342 Multiple rotations can be composed using the ``*`` operator:
344 >>> r1 = R.from_euler('z', 90, degrees=True)
345 >>> r2 = R.from_rotvec([np.pi/4, 0, 0])
346 >>> v = [1, 2, 3]
347 >>> r2.apply(r1.apply(v))
348 array([-2. , -1.41421356, 2.82842712])
349 >>> r3 = r2 * r1 # Note the order
350 >>> r3.apply(v)
351 array([-2. , -1.41421356, 2.82842712])
353 Finally, it is also possible to invert rotations:
355 >>> r1 = R.from_euler('z', [90, 45], degrees=True)
356 >>> r2 = r1.inv()
357 >>> r2.as_euler('zyx', degrees=True)
358 array([[-90., 0., 0.],
359 [-45., 0., 0.]])
361 These examples serve as an overview into the `Rotation` class and highlight
362 major functionalities. For more thorough examples of the range of input and
363 output formats supported, consult the individual method's examples.
365 """
366 def __init__(self, quat, normalize=True, copy=True):
367 self._single = False
368 quat = np.asarray(quat, dtype=float)
370 if quat.ndim not in [1, 2] or quat.shape[-1] != 4:
371 raise ValueError("Expected `quat` to have shape (4,) or (N x 4), "
372 "got {}.".format(quat.shape))
374 # If a single quaternion is given, convert it to a 2D 1 x 4 matrix but
375 # set self._single to True so that we can return appropriate objects
376 # in the `to_...` methods
377 if quat.shape == (4,):
378 quat = quat[None, :]
379 self._single = True
381 if normalize:
382 self._quat = quat.copy()
383 norms = scipy.linalg.norm(quat, axis=1)
385 zero_norms = norms == 0
386 if zero_norms.any():
387 raise ValueError("Found zero norm quaternions in `quat`.")
389 # Ensure norm is broadcasted along each column.
390 self._quat[~zero_norms] /= norms[~zero_norms][:, None]
391 else:
392 self._quat = quat.copy() if copy else quat
394 def __len__(self):
395 """Number of rotations contained in this object.
397 Multiple rotations can be stored in a single instance.
399 Returns
400 -------
401 length : int
402 Number of rotations stored in object.
404 """
405 return self._quat.shape[0]
407 @classmethod
408 def from_quat(cls, quat, normalized=None):
409 """Initialize from quaternions.
411 3D rotations can be represented using unit-norm quaternions [1]_.
413 Parameters
414 ----------
415 quat : array_like, shape (N, 4) or (4,)
416 Each row is a (possibly non-unit norm) quaternion in scalar-last
417 (x, y, z, w) format. Each quaternion will be normalized to unit
418 norm.
419 normalized
420 Deprecated argument. Has no effect, input `quat` is always
421 normalized.
423 .. deprecated:: 1.4.0
425 Returns
426 -------
427 rotation : `Rotation` instance
428 Object containing the rotations represented by input quaternions.
430 References
431 ----------
432 .. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
434 Examples
435 --------
436 >>> from scipy.spatial.transform import Rotation as R
438 Initialize a single rotation:
440 >>> r = R.from_quat([1, 0, 0, 0])
441 >>> r.as_quat()
442 array([1., 0., 0., 0.])
443 >>> r.as_quat().shape
444 (4,)
446 Initialize multiple rotations in a single object:
448 >>> r = R.from_quat([
449 ... [1, 0, 0, 0],
450 ... [0, 0, 0, 1]
451 ... ])
452 >>> r.as_quat()
453 array([[1., 0., 0., 0.],
454 [0., 0., 0., 1.]])
455 >>> r.as_quat().shape
456 (2, 4)
458 It is also possible to have a stack of a single rotation:
460 >>> r = R.from_quat([[0, 0, 0, 1]])
461 >>> r.as_quat()
462 array([[0., 0., 0., 1.]])
463 >>> r.as_quat().shape
464 (1, 4)
466 Quaternions are normalized before initialization.
468 >>> r = R.from_quat([0, 0, 1, 1])
469 >>> r.as_quat()
470 array([0. , 0. , 0.70710678, 0.70710678])
471 """
472 if normalized is not None:
473 warnings.warn("`normalized` is deprecated in scipy 1.4.0 and "
474 "will be removed in scipy 1.6.0. The input `quat` "
475 "is always normalized.", DeprecationWarning)
477 return cls(quat, normalize=True)
479 @classmethod
480 def from_matrix(cls, matrix):
481 """Initialize from rotation matrix.
483 Rotations in 3 dimensions can be represented with 3 x 3 proper
484 orthogonal matrices [1]_. If the input is not proper orthogonal,
485 an approximation is created using the method described in [2]_.
487 Parameters
488 ----------
489 matrix : array_like, shape (N, 3, 3) or (3, 3)
490 A single matrix or a stack of matrices, where ``matrix[i]`` is
491 the i-th matrix.
493 Returns
494 -------
495 rotation : `Rotation` instance
496 Object containing the rotations represented by the rotation
497 matrices.
499 References
500 ----------
501 .. [1] https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions
502 .. [2] F. Landis Markley, "Unit Quaternion from Rotation Matrix",
503 Journal of guidance, control, and dynamics vol. 31.2, pp.
504 440-442, 2008.
506 Examples
507 --------
508 >>> from scipy.spatial.transform import Rotation as R
510 Initialize a single rotation:
512 >>> r = R.from_matrix([
513 ... [0, -1, 0],
514 ... [1, 0, 0],
515 ... [0, 0, 1]])
516 >>> r.as_matrix().shape
517 (3, 3)
519 Initialize multiple rotations in a single object:
521 >>> r = R.from_matrix([
522 ... [
523 ... [0, -1, 0],
524 ... [1, 0, 0],
525 ... [0, 0, 1],
526 ... ],
527 ... [
528 ... [1, 0, 0],
529 ... [0, 0, -1],
530 ... [0, 1, 0],
531 ... ]])
532 >>> r.as_matrix().shape
533 (2, 3, 3)
535 If input matrices are not special orthogonal (orthogonal with
536 determinant equal to +1), then a special orthogonal estimate is stored:
538 >>> a = np.array([
539 ... [0, -0.5, 0],
540 ... [0.5, 0, 0],
541 ... [0, 0, 0.5]])
542 >>> np.linalg.det(a)
543 0.12500000000000003
544 >>> r = R.from_matrix(a)
545 >>> matrix = r.as_matrix()
546 >>> matrix
547 array([[-0.38461538, -0.92307692, 0. ],
548 [ 0.92307692, -0.38461538, 0. ],
549 [ 0. , 0. , 1. ]])
550 >>> np.linalg.det(matrix)
551 1.0000000000000002
553 It is also possible to have a stack containing a single rotation:
555 >>> r = R.from_matrix([[
556 ... [0, -1, 0],
557 ... [1, 0, 0],
558 ... [0, 0, 1]]])
559 >>> r.as_matrix()
560 array([[[ 0., -1., 0.],
561 [ 1., 0., 0.],
562 [ 0., 0., 1.]]])
563 >>> r.as_matrix().shape
564 (1, 3, 3)
566 Notes
567 -----
568 This function was called from_dcm before.
570 .. versionadded:: 1.4.0
571 """
572 is_single = False
573 matrix = np.asarray(matrix, dtype=float)
575 if matrix.ndim not in [2, 3] or matrix.shape[-2:] != (3, 3):
576 raise ValueError("Expected `matrix` to have shape (3, 3) or "
577 "(N, 3, 3), got {}".format(matrix.shape))
579 # If a single matrix is given, convert it to 3D 1 x 3 x 3 matrix but
580 # set self._single to True so that we can return appropriate objects in
581 # the `to_...` methods
582 if matrix.shape == (3, 3):
583 matrix = matrix.reshape((1, 3, 3))
584 is_single = True
586 num_rotations = matrix.shape[0]
588 decision_matrix = np.empty((num_rotations, 4))
589 decision_matrix[:, :3] = matrix.diagonal(axis1=1, axis2=2)
590 decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1)
591 choices = decision_matrix.argmax(axis=1)
593 quat = np.empty((num_rotations, 4))
595 ind = np.nonzero(choices != 3)[0]
596 i = choices[ind]
597 j = (i + 1) % 3
598 k = (j + 1) % 3
600 quat[ind, i] = 1 - decision_matrix[ind, -1] + 2 * matrix[ind, i, i]
601 quat[ind, j] = matrix[ind, j, i] + matrix[ind, i, j]
602 quat[ind, k] = matrix[ind, k, i] + matrix[ind, i, k]
603 quat[ind, 3] = matrix[ind, k, j] - matrix[ind, j, k]
605 ind = np.nonzero(choices == 3)[0]
606 quat[ind, 0] = matrix[ind, 2, 1] - matrix[ind, 1, 2]
607 quat[ind, 1] = matrix[ind, 0, 2] - matrix[ind, 2, 0]
608 quat[ind, 2] = matrix[ind, 1, 0] - matrix[ind, 0, 1]
609 quat[ind, 3] = 1 + decision_matrix[ind, -1]
611 quat /= np.linalg.norm(quat, axis=1)[:, None]
613 if is_single:
614 return cls(quat[0], normalize=False, copy=False)
615 else:
616 return cls(quat, normalize=False, copy=False)
618 @classmethod
619 @np.deprecate(message="from_dcm is renamed to from_matrix in scipy 1.4.0 "
620 "and will be removed in scipy 1.6.0")
621 def from_dcm(cls, dcm):
622 return cls.from_matrix(dcm)
624 @classmethod
625 def from_rotvec(cls, rotvec):
626 """Initialize from rotation vectors.
628 A rotation vector is a 3 dimensional vector which is co-directional to
629 the axis of rotation and whose norm gives the angle of rotation (in
630 radians) [1]_.
632 Parameters
633 ----------
634 rotvec : array_like, shape (N, 3) or (3,)
635 A single vector or a stack of vectors, where `rot_vec[i]` gives
636 the ith rotation vector.
638 Returns
639 -------
640 rotation : `Rotation` instance
641 Object containing the rotations represented by input rotation
642 vectors.
644 References
645 ----------
646 .. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector
648 Examples
649 --------
650 >>> from scipy.spatial.transform import Rotation as R
652 Initialize a single rotation:
654 >>> r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1]))
655 >>> r.as_rotvec()
656 array([0. , 0. , 1.57079633])
657 >>> r.as_rotvec().shape
658 (3,)
660 Initialize multiple rotations in one object:
662 >>> r = R.from_rotvec([
663 ... [0, 0, np.pi/2],
664 ... [np.pi/2, 0, 0]])
665 >>> r.as_rotvec()
666 array([[0. , 0. , 1.57079633],
667 [1.57079633, 0. , 0. ]])
668 >>> r.as_rotvec().shape
669 (2, 3)
671 It is also possible to have a stack of a single rotaton:
673 >>> r = R.from_rotvec([[0, 0, np.pi/2]])
674 >>> r.as_rotvec().shape
675 (1, 3)
677 """
678 is_single = False
679 rotvec = np.asarray(rotvec, dtype=float)
681 if rotvec.ndim not in [1, 2] or rotvec.shape[-1] != 3:
682 raise ValueError("Expected `rot_vec` to have shape (3,) "
683 "or (N, 3), got {}".format(rotvec.shape))
685 # If a single vector is given, convert it to a 2D 1 x 3 matrix but
686 # set self._single to True so that we can return appropriate objects
687 # in the `as_...` methods
688 if rotvec.shape == (3,):
689 rotvec = rotvec[None, :]
690 is_single = True
692 num_rotations = rotvec.shape[0]
694 norms = np.linalg.norm(rotvec, axis=1)
695 small_angle = (norms <= 1e-3)
696 large_angle = ~small_angle
698 scale = np.empty(num_rotations)
699 scale[small_angle] = (0.5 - norms[small_angle] ** 2 / 48 +
700 norms[small_angle] ** 4 / 3840)
701 scale[large_angle] = (np.sin(norms[large_angle] / 2) /
702 norms[large_angle])
704 quat = np.empty((num_rotations, 4))
705 quat[:, :3] = scale[:, None] * rotvec
706 quat[:, 3] = np.cos(norms / 2)
708 if is_single:
709 return cls(quat[0], normalize=False, copy=False)
710 else:
711 return cls(quat, normalize=False, copy=False)
713 @classmethod
714 def from_euler(cls, seq, angles, degrees=False):
715 """Initialize from Euler angles.
717 Rotations in 3-D can be represented by a sequence of 3
718 rotations around a sequence of axes. In theory, any three axes spanning
719 the 3-D Euclidean space are enough. In practice, the axes of rotation are
720 chosen to be the basis vectors.
722 The three rotations can either be in a global frame of reference
723 (extrinsic) or in a body centred frame of reference (intrinsic), which
724 is attached to, and moves with, the object under rotation [1]_.
726 Parameters
727 ----------
728 seq : string
729 Specifies sequence of axes for rotations. Up to 3 characters
730 belonging to the set {'X', 'Y', 'Z'} for intrinsic rotations, or
731 {'x', 'y', 'z'} for extrinsic rotations. Extrinsic and intrinsic
732 rotations cannot be mixed in one function call.
733 angles : float or array_like, shape (N,) or (N, [1 or 2 or 3])
734 Euler angles specified in radians (`degrees` is False) or degrees
735 (`degrees` is True).
736 For a single character `seq`, `angles` can be:
738 - a single value
739 - array_like with shape (N,), where each `angle[i]`
740 corresponds to a single rotation
741 - array_like with shape (N, 1), where each `angle[i, 0]`
742 corresponds to a single rotation
744 For 2- and 3-character wide `seq`, `angles` can be:
746 - array_like with shape (W,) where `W` is the width of
747 `seq`, which corresponds to a single rotation with `W` axes
748 - array_like with shape (N, W) where each `angle[i]`
749 corresponds to a sequence of Euler angles describing a single
750 rotation
752 degrees : bool, optional
753 If True, then the given angles are assumed to be in degrees.
754 Default is False.
756 Returns
757 -------
758 rotation : `Rotation` instance
759 Object containing the rotation represented by the sequence of
760 rotations around given axes with given angles.
762 References
763 ----------
764 .. [1] https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations
766 Examples
767 --------
768 >>> from scipy.spatial.transform import Rotation as R
770 Initialize a single rotation along a single axis:
772 >>> r = R.from_euler('x', 90, degrees=True)
773 >>> r.as_quat().shape
774 (4,)
776 Initialize a single rotation with a given axis sequence:
778 >>> r = R.from_euler('zyx', [90, 45, 30], degrees=True)
779 >>> r.as_quat().shape
780 (4,)
782 Initialize a stack with a single rotation around a single axis:
784 >>> r = R.from_euler('x', [90], degrees=True)
785 >>> r.as_quat().shape
786 (1, 4)
788 Initialize a stack with a single rotation with an axis sequence:
790 >>> r = R.from_euler('zyx', [[90, 45, 30]], degrees=True)
791 >>> r.as_quat().shape
792 (1, 4)
794 Initialize multiple elementary rotations in one object:
796 >>> r = R.from_euler('x', [90, 45, 30], degrees=True)
797 >>> r.as_quat().shape
798 (3, 4)
800 Initialize multiple rotations in one object:
802 >>> r = R.from_euler('zyx', [[90, 45, 30], [35, 45, 90]], degrees=True)
803 >>> r.as_quat().shape
804 (2, 4)
806 """
807 num_axes = len(seq)
808 if num_axes < 1 or num_axes > 3:
809 raise ValueError("Expected axis specification to be a non-empty "
810 "string of upto 3 characters, got {}".format(seq))
812 intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None)
813 extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None)
814 if not (intrinsic or extrinsic):
815 raise ValueError("Expected axes from `seq` to be from ['x', 'y', "
816 "'z'] or ['X', 'Y', 'Z'], got {}".format(seq))
818 if any(seq[i] == seq[i+1] for i in range(num_axes - 1)):
819 raise ValueError("Expected consecutive axes to be different, "
820 "got {}".format(seq))
822 seq = seq.lower()
824 angles = np.asarray(angles, dtype=float)
825 if degrees:
826 angles = np.deg2rad(angles)
828 is_single = False
829 # Prepare angles to have shape (num_rot, num_axes)
830 if num_axes == 1:
831 if angles.ndim == 0:
832 # (1, 1)
833 angles = angles.reshape((1, 1))
834 is_single = True
835 elif angles.ndim == 1:
836 # (N, 1)
837 angles = angles[:, None]
838 elif angles.ndim == 2 and angles.shape[-1] != 1:
839 raise ValueError("Expected `angles` parameter to have shape "
840 "(N, 1), got {}.".format(angles.shape))
841 elif angles.ndim > 2:
842 raise ValueError("Expected float, 1D array, or 2D array for "
843 "parameter `angles` corresponding to `seq`, "
844 "got shape {}.".format(angles.shape))
845 else: # 2 or 3 axes
846 if angles.ndim not in [1, 2] or angles.shape[-1] != num_axes:
847 raise ValueError("Expected `angles` to be at most "
848 "2-dimensional with width equal to number "
849 "of axes specified, got {} for shape".format(
850 angles.shape))
852 if angles.ndim == 1:
853 # (1, num_axes)
854 angles = angles[None, :]
855 is_single = True
857 # By now angles should have shape (num_rot, num_axes)
858 # sanity check
859 if angles.ndim != 2 or angles.shape[-1] != num_axes:
860 raise ValueError("Expected angles to have shape (num_rotations, "
861 "num_axes), got {}.".format(angles.shape))
863 quat = _elementary_quat_compose(seq, angles, intrinsic)
864 return cls(quat[0] if is_single else quat, normalize=False, copy=False)
866 def as_quat(self):
867 """Represent as quaternions.
869 Rotations in 3 dimensions can be represented using unit norm
870 quaternions [1]_. The mapping from quaternions to rotations is
871 two-to-one, i.e. quaternions ``q`` and ``-q``, where ``-q`` simply
872 reverses the sign of each component, represent the same spatial
873 rotation.
875 Returns
876 -------
877 quat : `numpy.ndarray`, shape (4,) or (N, 4)
878 Shape depends on shape of inputs used for initialization.
880 References
881 ----------
882 .. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
884 Examples
885 --------
886 >>> from scipy.spatial.transform import Rotation as R
888 Represent a single rotation:
890 >>> r = R.from_matrix([[0, -1, 0],
891 ... [1, 0, 0],
892 ... [0, 0, 1]])
893 >>> r.as_quat()
894 array([0. , 0. , 0.70710678, 0.70710678])
895 >>> r.as_quat().shape
896 (4,)
898 Represent a stack with a single rotation:
900 >>> r = R.from_quat([[0, 0, 0, 1]])
901 >>> r.as_quat().shape
902 (1, 4)
904 Represent multiple rotations in a single object:
906 >>> r = R.from_rotvec([[np.pi, 0, 0], [0, 0, np.pi/2]])
907 >>> r.as_quat().shape
908 (2, 4)
910 """
911 if self._single:
912 return self._quat[0].copy()
913 else:
914 return self._quat.copy()
916 def as_matrix(self):
917 """Represent as rotation matrix.
919 3D rotations can be represented using rotation matrices, which
920 are 3 x 3 real orthogonal matrices with determinant equal to +1 [1]_.
922 Returns
923 -------
924 matrix : ndarray, shape (3, 3) or (N, 3, 3)
925 Shape depends on shape of inputs used for initialization.
927 References
928 ----------
929 .. [1] https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions
931 Examples
932 --------
933 >>> from scipy.spatial.transform import Rotation as R
935 Represent a single rotation:
937 >>> r = R.from_rotvec([0, 0, np.pi/2])
938 >>> r.as_matrix()
939 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
940 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
941 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
942 >>> r.as_matrix().shape
943 (3, 3)
945 Represent a stack with a single rotation:
947 >>> r = R.from_quat([[1, 1, 0, 0]])
948 >>> r.as_matrix()
949 array([[[ 0., 1., 0.],
950 [ 1., 0., 0.],
951 [ 0., 0., -1.]]])
952 >>> r.as_matrix().shape
953 (1, 3, 3)
955 Represent multiple rotations:
957 >>> r = R.from_rotvec([[np.pi/2, 0, 0], [0, 0, np.pi/2]])
958 >>> r.as_matrix()
959 array([[[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00],
960 [ 0.00000000e+00, 2.22044605e-16, -1.00000000e+00],
961 [ 0.00000000e+00, 1.00000000e+00, 2.22044605e-16]],
962 [[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
963 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
964 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]])
965 >>> r.as_matrix().shape
966 (2, 3, 3)
968 Notes
969 -----
970 This function was called as_dcm before.
972 .. versionadded:: 1.4.0
973 """
974 x = self._quat[:, 0]
975 y = self._quat[:, 1]
976 z = self._quat[:, 2]
977 w = self._quat[:, 3]
979 x2 = x * x
980 y2 = y * y
981 z2 = z * z
982 w2 = w * w
984 xy = x * y
985 zw = z * w
986 xz = x * z
987 yw = y * w
988 yz = y * z
989 xw = x * w
991 num_rotations = len(self)
992 matrix = np.empty((num_rotations, 3, 3))
994 matrix[:, 0, 0] = x2 - y2 - z2 + w2
995 matrix[:, 1, 0] = 2 * (xy + zw)
996 matrix[:, 2, 0] = 2 * (xz - yw)
998 matrix[:, 0, 1] = 2 * (xy - zw)
999 matrix[:, 1, 1] = - x2 + y2 - z2 + w2
1000 matrix[:, 2, 1] = 2 * (yz + xw)
1002 matrix[:, 0, 2] = 2 * (xz + yw)
1003 matrix[:, 1, 2] = 2 * (yz - xw)
1004 matrix[:, 2, 2] = - x2 - y2 + z2 + w2
1006 if self._single:
1007 return matrix[0]
1008 else:
1009 return matrix
1011 @np.deprecate(message="as_dcm is renamed to as_matrix in scipy 1.4.0 "
1012 "and will be removed in scipy 1.6.0")
1013 def as_dcm(self):
1014 return self.as_matrix()
1016 def as_rotvec(self):
1017 """Represent as rotation vectors.
1019 A rotation vector is a 3 dimensional vector which is co-directional to
1020 the axis of rotation and whose norm gives the angle of rotation (in
1021 radians) [1]_.
1023 Returns
1024 -------
1025 rotvec : ndarray, shape (3,) or (N, 3)
1026 Shape depends on shape of inputs used for initialization.
1028 References
1029 ----------
1030 .. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector
1032 Examples
1033 --------
1034 >>> from scipy.spatial.transform import Rotation as R
1036 Represent a single rotation:
1038 >>> r = R.from_euler('z', 90, degrees=True)
1039 >>> r.as_rotvec()
1040 array([0. , 0. , 1.57079633])
1041 >>> r.as_rotvec().shape
1042 (3,)
1044 Represent a stack with a single rotation:
1046 >>> r = R.from_quat([[0, 0, 1, 1]])
1047 >>> r.as_rotvec()
1048 array([[0. , 0. , 1.57079633]])
1049 >>> r.as_rotvec().shape
1050 (1, 3)
1052 Represent multiple rotations in a single object:
1054 >>> r = R.from_quat([[0, 0, 1, 1], [1, 1, 0, 1]])
1055 >>> r.as_rotvec()
1056 array([[0. , 0. , 1.57079633],
1057 [1.35102172, 1.35102172, 0. ]])
1058 >>> r.as_rotvec().shape
1059 (2, 3)
1061 """
1062 quat = self._quat.copy()
1063 # w > 0 to ensure 0 <= angle <= pi
1064 quat[quat[:, 3] < 0] *= -1
1066 angle = 2 * np.arctan2(np.linalg.norm(quat[:, :3], axis=1), quat[:, 3])
1068 small_angle = (angle <= 1e-3)
1069 large_angle = ~small_angle
1071 num_rotations = len(self)
1072 scale = np.empty(num_rotations)
1073 scale[small_angle] = (2 + angle[small_angle] ** 2 / 12 +
1074 7 * angle[small_angle] ** 4 / 2880)
1075 scale[large_angle] = (angle[large_angle] /
1076 np.sin(angle[large_angle] / 2))
1078 rotvec = scale[:, None] * quat[:, :3]
1080 if self._single:
1081 return rotvec[0]
1082 else:
1083 return rotvec
1085 def as_euler(self, seq, degrees=False):
1086 """Represent as Euler angles.
1088 Any orientation can be expressed as a composition of 3 elementary
1089 rotations. Once the axis sequence has been chosen, Euler angles define
1090 the angle of rotation around each respective axis [1]_.
1092 The algorithm from [2]_ has been used to calculate Euler angles for the
1093 rotation about a given sequence of axes.
1095 Euler angles suffer from the problem of gimbal lock [3]_, where the
1096 representation loses a degree of freedom and it is not possible to
1097 determine the first and third angles uniquely. In this case,
1098 a warning is raised, and the third angle is set to zero. Note however
1099 that the returned angles still represent the correct rotation.
1101 Parameters
1102 ----------
1103 seq : string, length 3
1104 3 characters belonging to the set {'X', 'Y', 'Z'} for intrinsic
1105 rotations, or {'x', 'y', 'z'} for extrinsic rotations [1]_.
1106 Adjacent axes cannot be the same.
1107 Extrinsic and intrinsic rotations cannot be mixed in one function
1108 call.
1109 degrees : boolean, optional
1110 Returned angles are in degrees if this flag is True, else they are
1111 in radians. Default is False.
1113 Returns
1114 -------
1115 angles : ndarray, shape (3,) or (N, 3)
1116 Shape depends on shape of inputs used to initialize object.
1117 The returned angles are in the range:
1119 - First angle belongs to [-180, 180] degrees (both inclusive)
1120 - Third angle belongs to [-180, 180] degrees (both inclusive)
1121 - Second angle belongs to:
1123 - [-90, 90] degrees if all axes are different (like xyz)
1124 - [0, 180] degrees if first and third axes are the same
1125 (like zxz)
1127 References
1128 ----------
1129 .. [1] https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations
1130 .. [2] Malcolm D. Shuster, F. Landis Markley, "General formula for
1131 extraction the Euler angles", Journal of guidance, control, and
1132 dynamics, vol. 29.1, pp. 215-221. 2006
1133 .. [3] https://en.wikipedia.org/wiki/Gimbal_lock#In_applied_mathematics
1135 Examples
1136 --------
1137 >>> from scipy.spatial.transform import Rotation as R
1139 Represent a single rotation:
1141 >>> r = R.from_rotvec([0, 0, np.pi/2])
1142 >>> r.as_euler('zxy', degrees=True)
1143 array([90., 0., 0.])
1144 >>> r.as_euler('zxy', degrees=True).shape
1145 (3,)
1147 Represent a stack of single rotation:
1149 >>> r = R.from_rotvec([[0, 0, np.pi/2]])
1150 >>> r.as_euler('zxy', degrees=True)
1151 array([[90., 0., 0.]])
1152 >>> r.as_euler('zxy', degrees=True).shape
1153 (1, 3)
1155 Represent multiple rotations in a single object:
1157 >>> r = R.from_rotvec([
1158 ... [0, 0, np.pi/2],
1159 ... [0, -np.pi/3, 0],
1160 ... [np.pi/4, 0, 0]])
1161 >>> r.as_euler('zxy', degrees=True)
1162 array([[ 90., 0., 0.],
1163 [ 0., 0., -60.],
1164 [ 0., 45., 0.]])
1165 >>> r.as_euler('zxy', degrees=True).shape
1166 (3, 3)
1168 """
1169 if len(seq) != 3:
1170 raise ValueError("Expected 3 axes, got {}.".format(seq))
1172 intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None)
1173 extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None)
1174 if not (intrinsic or extrinsic):
1175 raise ValueError("Expected axes from `seq` to be from "
1176 "['x', 'y', 'z'] or ['X', 'Y', 'Z'], "
1177 "got {}".format(seq))
1179 if any(seq[i] == seq[i+1] for i in range(2)):
1180 raise ValueError("Expected consecutive axes to be different, "
1181 "got {}".format(seq))
1183 seq = seq.lower()
1185 angles = _compute_euler_from_matrix(self.as_matrix(), seq, extrinsic)
1186 if degrees:
1187 angles = np.rad2deg(angles)
1189 return angles[0] if self._single else angles
1191 def apply(self, vectors, inverse=False):
1192 """Apply this rotation to a set of vectors.
1194 If the original frame rotates to the final frame by this rotation, then
1195 its application to a vector can be seen in two ways:
1197 - As a projection of vector components expressed in the final frame
1198 to the original frame.
1199 - As the physical rotation of a vector being glued to the original
1200 frame as it rotates. In this case the vector components are
1201 expressed in the original frame before and after the rotation.
1203 In terms of rotation matricies, this application is the same as
1204 ``self.as_matrix().dot(vectors)``.
1206 Parameters
1207 ----------
1208 vectors : array_like, shape (3,) or (N, 3)
1209 Each `vectors[i]` represents a vector in 3D space. A single vector
1210 can either be specified with shape `(3, )` or `(1, 3)`. The number
1211 of rotations and number of vectors given must follow standard numpy
1212 broadcasting rules: either one of them equals unity or they both
1213 equal each other.
1214 inverse : boolean, optional
1215 If True then the inverse of the rotation(s) is applied to the input
1216 vectors. Default is False.
1218 Returns
1219 -------
1220 rotated_vectors : ndarray, shape (3,) or (N, 3)
1221 Result of applying rotation on input vectors.
1222 Shape depends on the following cases:
1224 - If object contains a single rotation (as opposed to a stack
1225 with a single rotation) and a single vector is specified with
1226 shape ``(3,)``, then `rotated_vectors` has shape ``(3,)``.
1227 - In all other cases, `rotated_vectors` has shape ``(N, 3)``,
1228 where ``N`` is either the number of rotations or vectors.
1230 Examples
1231 --------
1232 >>> from scipy.spatial.transform import Rotation as R
1234 Single rotation applied on a single vector:
1236 >>> vector = np.array([1, 0, 0])
1237 >>> r = R.from_rotvec([0, 0, np.pi/2])
1238 >>> r.as_matrix()
1239 array([[ 2.22044605e-16, -1.00000000e+00, 0.00000000e+00],
1240 [ 1.00000000e+00, 2.22044605e-16, 0.00000000e+00],
1241 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
1242 >>> r.apply(vector)
1243 array([2.22044605e-16, 1.00000000e+00, 0.00000000e+00])
1244 >>> r.apply(vector).shape
1245 (3,)
1247 Single rotation applied on multiple vectors:
1249 >>> vectors = np.array([
1250 ... [1, 0, 0],
1251 ... [1, 2, 3]])
1252 >>> r = R.from_rotvec([0, 0, np.pi/4])
1253 >>> r.as_matrix()
1254 array([[ 0.70710678, -0.70710678, 0. ],
1255 [ 0.70710678, 0.70710678, 0. ],
1256 [ 0. , 0. , 1. ]])
1257 >>> r.apply(vectors)
1258 array([[ 0.70710678, 0.70710678, 0. ],
1259 [-0.70710678, 2.12132034, 3. ]])
1260 >>> r.apply(vectors).shape
1261 (2, 3)
1263 Multiple rotations on a single vector:
1265 >>> r = R.from_rotvec([[0, 0, np.pi/4], [np.pi/2, 0, 0]])
1266 >>> vector = np.array([1,2,3])
1267 >>> r.as_matrix()
1268 array([[[ 7.07106781e-01, -7.07106781e-01, 0.00000000e+00],
1269 [ 7.07106781e-01, 7.07106781e-01, 0.00000000e+00],
1270 [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
1271 [[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00],
1272 [ 0.00000000e+00, 2.22044605e-16, -1.00000000e+00],
1273 [ 0.00000000e+00, 1.00000000e+00, 2.22044605e-16]]])
1274 >>> r.apply(vector)
1275 array([[-0.70710678, 2.12132034, 3. ],
1276 [ 1. , -3. , 2. ]])
1277 >>> r.apply(vector).shape
1278 (2, 3)
1280 Multiple rotations on multiple vectors. Each rotation is applied on the
1281 corresponding vector:
1283 >>> r = R.from_euler('zxy', [
1284 ... [0, 0, 90],
1285 ... [45, 30, 60]], degrees=True)
1286 >>> vectors = [
1287 ... [1, 2, 3],
1288 ... [1, 0, -1]]
1289 >>> r.apply(vectors)
1290 array([[ 3. , 2. , -1. ],
1291 [-0.09026039, 1.11237244, -0.86860844]])
1292 >>> r.apply(vectors).shape
1293 (2, 3)
1295 It is also possible to apply the inverse rotation:
1297 >>> r = R.from_euler('zxy', [
1298 ... [0, 0, 90],
1299 ... [45, 30, 60]], degrees=True)
1300 >>> vectors = [
1301 ... [1, 2, 3],
1302 ... [1, 0, -1]]
1303 >>> r.apply(vectors, inverse=True)
1304 array([[-3. , 2. , 1. ],
1305 [ 1.09533535, -0.8365163 , 0.3169873 ]])
1307 """
1308 vectors = np.asarray(vectors)
1309 if vectors.ndim > 2 or vectors.shape[-1] != 3:
1310 raise ValueError("Expected input of shape (3,) or (P, 3), "
1311 "got {}.".format(vectors.shape))
1313 single_vector = False
1314 if vectors.shape == (3,):
1315 single_vector = True
1316 vectors = vectors[None, :]
1318 matrix = self.as_matrix()
1319 if self._single:
1320 matrix = matrix[None, :, :]
1322 n_vectors = vectors.shape[0]
1323 n_rotations = len(self)
1325 if n_vectors != 1 and n_rotations != 1 and n_vectors != n_rotations:
1326 raise ValueError("Expected equal numbers of rotations and vectors "
1327 ", or a single rotation, or a single vector, got "
1328 "{} rotations and {} vectors.".format(
1329 n_rotations, n_vectors))
1331 if inverse:
1332 result = np.einsum('ikj,ik->ij', matrix, vectors)
1333 else:
1334 result = np.einsum('ijk,ik->ij', matrix, vectors)
1336 if self._single and single_vector:
1337 return result[0]
1338 else:
1339 return result
1341 def __mul__(self, other):
1342 """Compose this rotation with the other.
1344 If `p` and `q` are two rotations, then the composition of 'q followed
1345 by p' is equivalent to `p * q`. In terms of rotation matrices,
1346 the composition can be expressed as
1347 ``p.as_matrix().dot(q.as_matrix())``.
1349 Parameters
1350 ----------
1351 other : `Rotation` instance
1352 Object containing the rotations to be composed with this one. Note
1353 that rotation compositions are not commutative, so ``p * q`` is
1354 different from ``q * p``.
1356 Returns
1357 -------
1358 composition : `Rotation` instance
1359 This function supports composition of multiple rotations at a time.
1360 The following cases are possible:
1362 - Either ``p`` or ``q`` contains a single rotation. In this case
1363 `composition` contains the result of composing each rotation in
1364 the other object with the single rotation.
1365 - Both ``p`` and ``q`` contain ``N`` rotations. In this case each
1366 rotation ``p[i]`` is composed with the corresponding rotation
1367 ``q[i]`` and `output` contains ``N`` rotations.
1369 Examples
1370 --------
1371 >>> from scipy.spatial.transform import Rotation as R
1373 Composition of two single rotations:
1375 >>> p = R.from_quat([0, 0, 1, 1])
1376 >>> q = R.from_quat([1, 0, 0, 1])
1377 >>> p.as_matrix()
1378 array([[ 0., -1., 0.],
1379 [ 1., 0., 0.],
1380 [ 0., 0., 1.]])
1381 >>> q.as_matrix()
1382 array([[ 1., 0., 0.],
1383 [ 0., 0., -1.],
1384 [ 0., 1., 0.]])
1385 >>> r = p * q
1386 >>> r.as_matrix()
1387 array([[0., 0., 1.],
1388 [1., 0., 0.],
1389 [0., 1., 0.]])
1391 Composition of two objects containing equal number of rotations:
1393 >>> p = R.from_quat([[0, 0, 1, 1], [1, 0, 0, 1]])
1394 >>> q = R.from_rotvec([[np.pi/4, 0, 0], [-np.pi/4, 0, np.pi/4]])
1395 >>> p.as_quat()
1396 array([[0. , 0. , 0.70710678, 0.70710678],
1397 [0.70710678, 0. , 0. , 0.70710678]])
1398 >>> q.as_quat()
1399 array([[ 0.38268343, 0. , 0. , 0.92387953],
1400 [-0.37282173, 0. , 0.37282173, 0.84971049]])
1401 >>> r = p * q
1402 >>> r.as_quat()
1403 array([[ 0.27059805, 0.27059805, 0.65328148, 0.65328148],
1404 [ 0.33721128, -0.26362477, 0.26362477, 0.86446082]])
1406 """
1407 if not(len(self) == 1 or len(other) == 1 or len(self) == len(other)):
1408 raise ValueError("Expected equal number of rotations in both "
1409 "or a single rotation in either object, "
1410 "got {} rotations in first and {} rotations in "
1411 "second object.".format(
1412 len(self), len(other)))
1413 result = _compose_quat(self._quat, other._quat)
1414 if self._single and other._single:
1415 result = result[0]
1416 return self.__class__(result, normalize=True, copy=False)
1418 def inv(self):
1419 """Invert this rotation.
1421 Composition of a rotation with its inverse results in an identity
1422 transformation.
1424 Returns
1425 -------
1426 inverse : `Rotation` instance
1427 Object containing inverse of the rotations in the current instance.
1429 Examples
1430 --------
1431 >>> from scipy.spatial.transform import Rotation as R
1433 Inverting a single rotation:
1435 >>> p = R.from_euler('z', 45, degrees=True)
1436 >>> q = p.inv()
1437 >>> q.as_euler('zyx', degrees=True)
1438 array([-45., 0., 0.])
1440 Inverting multiple rotations:
1442 >>> p = R.from_rotvec([[0, 0, np.pi/3], [-np.pi/4, 0, 0]])
1443 >>> q = p.inv()
1444 >>> q.as_rotvec()
1445 array([[-0. , -0. , -1.04719755],
1446 [ 0.78539816, -0. , -0. ]])
1448 """
1449 quat = self._quat.copy()
1450 quat[:, -1] *= -1
1451 if self._single:
1452 quat = quat[0]
1453 return self.__class__(quat, normalize=False, copy=False)
1455 def magnitude(self):
1456 """Get the magnitude(s) of the rotation(s).
1458 Returns
1459 -------
1460 magnitude : ndarray or float
1461 Angle(s) in radians, float if object contains a single rotation
1462 and ndarray if object contains multiple rotations.
1464 Examples
1465 --------
1466 >>> from scipy.spatial.transform import Rotation as R
1467 >>> r = R.from_quat(np.eye(4))
1468 >>> r.magnitude()
1469 array([3.14159265, 3.14159265, 3.14159265, 0. ])
1471 Magnitude of a single rotation:
1473 >>> r[0].magnitude()
1474 3.141592653589793
1475 """
1477 quat = self._quat.reshape((len(self), 4))
1478 s = np.linalg.norm(quat[:, :3], axis=1)
1479 c = np.abs(quat[:, 3])
1480 angles = 2 * np.arctan2(s, c)
1482 if self._single:
1483 return angles[0]
1484 else:
1485 return angles
1487 def mean(self, weights=None):
1488 """Get the mean of the rotations.
1490 Parameters
1491 ----------
1492 weights : array_like shape (N,), optional
1493 Weights describing the relative importance of the rotations. If
1494 None (default), then all values in `weights` are assumed to be
1495 equal.
1497 Returns
1498 -------
1499 mean : `Rotation` instance
1500 Object containing the mean of the rotations in the current
1501 instance.
1503 Notes
1504 -----
1505 The mean used is the chordal L2 mean (also called the projected or
1506 induced arithmetic mean). If ``p`` is a set of rotations with mean
1507 ``m``, then ``m`` is the rotation which minimizes
1508 ``(weights[:, None, None] * (p.as_matrix() - m.as_matrix())**2).sum()``.
1510 Examples
1511 --------
1512 >>> from scipy.spatial.transform import Rotation as R
1513 >>> r = R.from_euler('zyx', [[0, 0, 0],
1514 ... [1, 0, 0],
1515 ... [0, 1, 0],
1516 ... [0, 0, 1]], degrees=True)
1517 >>> r.mean().as_euler('zyx', degrees=True)
1518 array([0.24945696, 0.25054542, 0.24945696])
1519 """
1520 if weights is None:
1521 weights = np.ones(len(self))
1522 else:
1523 weights = np.asarray(weights)
1524 if weights.ndim != 1:
1525 raise ValueError("Expected `weights` to be 1 dimensional, got "
1526 "shape {}.".format(weights.shape))
1527 if weights.shape[0] != len(self):
1528 raise ValueError("Expected `weights` to have number of values "
1529 "equal to number of rotations, got "
1530 "{} values and {} rotations.".format(
1531 weights.shape[0], len(self)))
1532 if np.any(weights < 0):
1533 raise ValueError("`weights` must be non-negative.")
1535 K = np.dot(weights * self._quat.T, self._quat)
1536 l, v = np.linalg.eigh(K)
1537 return self.__class__(v[:, -1], normalize=False)
1539 def reduce(self, left=None, right=None, return_indices=False):
1540 """Reduce this rotation with the provided rotation groups.
1542 Reduction of a rotation ``p`` is a transformation of the form
1543 ``q = l * p * r``, where ``l`` and ``r`` are chosen from `left` and
1544 `right` respectively, such that rotation ``q`` has the smallest
1545 magnitude.
1547 If `left` and `right` are rotation groups representing symmetries of
1548 two objects rotated by ``p``, then ``q`` is the rotation of the
1549 smallest magnitude to align these objects considering their symmetries.
1551 Parameters
1552 ----------
1553 left : `Rotation` instance, optional
1554 Object containing the left rotation(s). Default value (None)
1555 corresponds to the identity rotation.
1556 right : `Rotation` instance, optional
1557 Object containing the right rotation(s). Default value (None)
1558 corresponds to the identity rotation.
1559 return_indices : bool, optional
1560 Whether to return the indices of the rotations from `left` and
1561 `right` used for reduction.
1563 Returns
1564 -------
1565 reduced : `Rotation` instance
1566 Object containing reduced rotations.
1567 left_best, right_best: integer ndarray
1568 Indices of elements from `left` and `right` used for reduction.
1569 """
1570 if left is None and right is None:
1571 reduced = self.__class__(self._quat, normalize=False, copy=True)
1572 if return_indices:
1573 return reduced, None, None
1574 else:
1575 return reduced
1576 elif right is None:
1577 right = Rotation.identity()
1578 elif left is None:
1579 left = Rotation.identity()
1581 # Levi-Civita tensor for triple product computations
1582 e = np.zeros((3, 3, 3))
1583 e[0, 1, 2] = e[1, 2, 0] = e[2, 0, 1] = 1
1584 e[0, 2, 1] = e[2, 1, 0] = e[1, 0, 2] = -1
1586 # We want to calculate the real components of q = l * p * r. It can
1587 # be shown that:
1588 # qs = ls * ps * rs - ls * dot(pv, rv) - ps * dot(lv, rv)
1589 # - rs * dot(lv, pv) - dot(cross(lv, pv), rv)
1590 # where ls and lv denote the scalar and vector components of l.
1592 def split_rotation(R):
1593 q = np.atleast_2d(R.as_quat())
1594 return q[:, -1], q[:, :-1]
1596 p = self
1597 ps, pv = split_rotation(p)
1598 ls, lv = split_rotation(left)
1599 rs, rv = split_rotation(right)
1601 qs = np.abs(np.einsum('i,j,k', ls, ps, rs) -
1602 np.einsum('i,jx,kx', ls, pv, rv) -
1603 np.einsum('ix,j,kx', lv, ps, rv) -
1604 np.einsum('ix,jx,k', lv, pv, rs) -
1605 np.einsum('xyz,ix,jy,kz', e, lv, pv, rv))
1606 qs = np.reshape(np.rollaxis(qs, 1), (qs.shape[1], -1))
1608 # Find best indices from scalar components
1609 max_ind = np.argmax(np.reshape(qs, (len(qs), -1)), axis=1)
1610 left_best = max_ind // len(right)
1611 right_best = max_ind % len(right)
1613 # Reduce the rotation using the best indices
1614 reduced = left[left_best] * p * right[right_best]
1615 if self._single:
1616 reduced = reduced[0]
1617 left_best = left_best[0]
1618 right_best = right_best[0]
1620 if return_indices:
1621 if left is None:
1622 left_best = None
1623 if right is None:
1624 right_best = None
1625 return reduced, left_best, right_best
1626 else:
1627 return reduced
1629 @classmethod
1630 def create_group(cls, group, axis='Z'):
1631 """Create a 3D rotation group.
1633 Parameters
1634 ----------
1635 group : string
1636 The name of the group. Must be one of 'I', 'O', 'T', 'Dn', 'Cn',
1637 where `n` is a positive integer. The groups are:
1639 * I: Icosahedral group
1640 * O: Octahedral group
1641 * T: Tetrahedral group
1642 * D: Dicyclic group
1643 * C: Cyclic group
1645 axis : integer
1646 The cyclic rotation axis. Must be one of ['X', 'Y', 'Z'] (or
1647 lowercase). Default is 'Z'. Ignored for groups 'I', 'O', and 'T'.
1649 Returns
1650 -------
1651 rotation : `Rotation` instance
1652 Object containing the elements of the rotation group.
1654 Notes
1655 -----
1656 This method generates rotation groups only. The full 3-dimensional
1657 point groups [PointGroups]_ also contain reflections.
1659 References
1660 ----------
1661 .. [PointGroups] `Point groups
1662 <https://en.wikipedia.org/wiki/Point_groups_in_three_dimensions>`_
1663 on Wikipedia.
1664 """
1665 return create_group(cls, group, axis=axis)
1667 def __getitem__(self, indexer):
1668 """Extract rotation(s) at given index(es) from object.
1670 Create a new `Rotation` instance containing a subset of rotations
1671 stored in this object.
1673 Parameters
1674 ----------
1675 indexer : index, slice, or index array
1676 Specifies which rotation(s) to extract. A single indexer must be
1677 specified, i.e. as if indexing a 1 dimensional array or list.
1679 Returns
1680 -------
1681 rotation : `Rotation` instance
1682 Contains
1683 - a single rotation, if `indexer` is a single index
1684 - a stack of rotation(s), if `indexer` is a slice, or and index
1685 array.
1687 Examples
1688 --------
1689 >>> from scipy.spatial.transform import Rotation as R
1690 >>> r = R.from_quat([
1691 ... [1, 1, 0, 0],
1692 ... [0, 1, 0, 1],
1693 ... [1, 1, -1, 0]])
1694 >>> r.as_quat()
1695 array([[ 0.70710678, 0.70710678, 0. , 0. ],
1696 [ 0. , 0.70710678, 0. , 0.70710678],
1697 [ 0.57735027, 0.57735027, -0.57735027, 0. ]])
1699 Indexing using a single index:
1701 >>> p = r[0]
1702 >>> p.as_quat()
1703 array([0.70710678, 0.70710678, 0. , 0. ])
1705 Array slicing:
1707 >>> q = r[1:3]
1708 >>> q.as_quat()
1709 array([[ 0. , 0.70710678, 0. , 0.70710678],
1710 [ 0.57735027, 0.57735027, -0.57735027, 0. ]])
1712 """
1713 return self.__class__(self._quat[indexer], normalize=False)
1715 @classmethod
1716 def identity(cls, num=None):
1717 """Get identity rotation(s).
1719 Composition with the identity rotation has no effect.
1721 Parameters
1722 ----------
1723 num : int or None, optional
1724 Number of identity rotations to generate. If None (default), then a
1725 single rotation is generated.
1727 Returns
1728 -------
1729 identity : Rotation object
1730 The identity rotation.
1731 """
1732 if num is None:
1733 q = [0, 0, 0, 1]
1734 else:
1735 q = np.zeros((num, 4))
1736 q[:, 3] = 1
1737 return cls(q, normalize=False)
1739 @classmethod
1740 def random(cls, num=None, random_state=None):
1741 """Generate uniformly distributed rotations.
1743 Parameters
1744 ----------
1745 num : int or None, optional
1746 Number of random rotations to generate. If None (default), then a
1747 single rotation is generated.
1748 random_state : int, RandomState instance or None, optional
1749 Accepts an integer as a seed for the random generator or a
1750 RandomState object. If None (default), uses global `numpy.random`
1751 random state.
1753 Returns
1754 -------
1755 random_rotation : `Rotation` instance
1756 Contains a single rotation if `num` is None. Otherwise contains a
1757 stack of `num` rotations.
1759 Examples
1760 --------
1761 >>> from scipy.spatial.transform import Rotation as R
1763 Sample a single rotation:
1765 >>> R.random(random_state=1234).as_euler('zxy', degrees=True)
1766 array([-110.5976185 , 55.32758512, 76.3289269 ])
1768 Sample a stack of rotations:
1770 >>> R.random(5, random_state=1234).as_euler('zxy', degrees=True)
1771 array([[-110.5976185 , 55.32758512, 76.3289269 ],
1772 [ -91.59132005, -14.3629884 , -93.91933182],
1773 [ 25.23835501, 45.02035145, -121.67867086],
1774 [ -51.51414184, -15.29022692, -172.46870023],
1775 [ -81.63376847, -27.39521579, 2.60408416]])
1777 """
1778 random_state = check_random_state(random_state)
1780 if num is None:
1781 sample = random_state.normal(size=4)
1782 else:
1783 sample = random_state.normal(size=(num, 4))
1785 return cls(sample)
1787 @classmethod
1788 @np.deprecate(message="match_vectors is deprecated in favor of "
1789 "align_vectors in scipy 1.4.0 and will be removed "
1790 "in scipy 1.6.0")
1791 def match_vectors(cls, a, b, weights=None, normalized=False):
1792 """Deprecated in favor of `align_vectors`."""
1793 a = np.asarray(a)
1794 if a.ndim != 2 or a.shape[-1] != 3:
1795 raise ValueError("Expected input `a` to have shape (N, 3), "
1796 "got {}".format(a.shape))
1797 b = np.asarray(b)
1798 if b.ndim != 2 or b.shape[-1] != 3:
1799 raise ValueError("Expected input `b` to have shape (N, 3), "
1800 "got {}.".format(b.shape))
1802 if a.shape != b.shape:
1803 raise ValueError("Expected inputs `a` and `b` to have same shapes"
1804 ", got {} and {} respectively.".format(
1805 a.shape, b.shape))
1807 if b.shape[0] == 1:
1808 raise ValueError("Rotation cannot be estimated using a single "
1809 "vector.")
1811 if weights is None:
1812 weights = np.ones(b.shape[0])
1813 else:
1814 weights = np.asarray(weights)
1815 if weights.ndim != 1:
1816 raise ValueError("Expected `weights` to be 1 dimensional, got "
1817 "shape {}.".format(weights.shape))
1818 if weights.shape[0] != b.shape[0]:
1819 raise ValueError("Expected `weights` to have number of values "
1820 "equal to number of input vectors, got "
1821 "{} values and {} vectors.".format(
1822 weights.shape[0], b.shape[0]))
1823 weights = weights / np.sum(weights)
1825 if not normalized:
1826 a = a / scipy.linalg.norm(a, axis=1)[:, None]
1827 b = b / scipy.linalg.norm(b, axis=1)[:, None]
1829 B = np.einsum('ji,jk->ik', weights[:, None] * a, b)
1830 u, s, vh = np.linalg.svd(B)
1832 # Correct improper rotation if necessary (as in Kabsch algorithm)
1833 if np.linalg.det(u @ vh) < 0:
1834 s[-1] = -s[-1]
1835 u[:, -1] = -u[:, -1]
1837 C = np.dot(u, vh)
1839 zeta = (s[0]+s[1]) * (s[1]+s[2]) * (s[2]+s[0])
1840 if np.abs(zeta) <= 1e-16:
1841 raise ValueError("Three component error vector has infinite "
1842 "covariance. It is impossible to determine the "
1843 "rotation uniquely.")
1845 kappa = s[0]*s[1] + s[1]*s[2] + s[2]*s[0]
1846 sensitivity = ((kappa * np.eye(3) + np.dot(B, B.T)) /
1847 (zeta * a.shape[0]))
1848 return cls.from_matrix(C), sensitivity
1850 @classmethod
1851 def align_vectors(cls, a, b, weights=None, return_sensitivity=False):
1852 """Estimate a rotation to optimally align two sets of vectors.
1854 Find a rotation between frames A and B which best aligns a set of
1855 vectors `a` and `b` observed in these frames. The following loss
1856 function is minimized to solve for the rotation matrix
1857 :math:`C`:
1859 .. math::
1861 L(C) = \\frac{1}{2} \\sum_{i = 1}^{n} w_i \\lVert \\mathbf{a}_i -
1862 C \\mathbf{b}_i \\rVert^2 ,
1864 where :math:`w_i`'s are the `weights` corresponding to each vector.
1866 The rotation is estimated with Kabsch algorithm [1]_.
1868 Parameters
1869 ----------
1870 a : array_like, shape (N, 3)
1871 Vector components observed in initial frame A. Each row of `a`
1872 denotes a vector.
1873 b : array_like, shape (N, 3)
1874 Vector components observed in another frame B. Each row of `b`
1875 denotes a vector.
1876 weights : array_like shape (N,), optional
1877 Weights describing the relative importance of the vector
1878 observations. If None (default), then all values in `weights` are
1879 assumed to be 1.
1880 return_sensitivity : bool, optional
1881 Whether to return the sensitivity matrix. See Notes for details.
1882 Default is False.
1884 Returns
1885 -------
1886 estimated_rotation : `Rotation` instance
1887 Best estimate of the rotation that transforms `b` to `a`.
1888 rmsd : float
1889 Root mean square distance (weighted) between the given set of
1890 vectors after alignment. It is equal to ``sqrt(2 * minimum_loss)``,
1891 where ``minimum_loss`` is the loss function evaluated for the
1892 found optimal rotation.
1893 sensitivity_matrix : ndarray, shape (3, 3)
1894 Sensitivity matrix of the estimated rotation estimate as explained
1895 in Notes. Returned only when `return_sensitivity` is True.
1897 Notes
1898 -----
1899 This method can also compute the sensitivity of the estimated rotation
1900 to small perturbations of the vector measurements. Specifically we
1901 consider the rotation estimate error as a small rotation vector of
1902 frame A. The sensitivity matrix is proportional to the covariance of
1903 this rotation vector assuming that the vectors in `a` was measured with
1904 errors significantly less than their lengths. To get the true
1905 covariance matrix, the returned sensitivity matrix must be multiplied
1906 by harmonic mean [3]_ of variance in each observation. Note that
1907 `weights` are supposed to be inversely proportional to the observation
1908 variances to get consistent results. For example, if all vectors are
1909 measured with the same accuracy of 0.01 (`weights` must be all equal),
1910 then you should multiple the sensitivity matrix by 0.01**2 to get the
1911 covariance.
1913 Refer to [2]_ for more rigorous discussion of the covariance
1914 estimation.
1916 References
1917 ----------
1918 .. [1] https://en.wikipedia.org/wiki/Kabsch_algorithm
1919 .. [2] F. Landis Markley,
1920 "Attitude determination using vector observations: a fast
1921 optimal matrix algorithm", Journal of Astronautical Sciences,
1922 Vol. 41, No.2, 1993, pp. 261-280.
1923 .. [3] https://en.wikipedia.org/wiki/Harmonic_mean
1924 """
1925 a = np.asarray(a)
1926 if a.ndim != 2 or a.shape[-1] != 3:
1927 raise ValueError("Expected input `a` to have shape (N, 3), "
1928 "got {}".format(a.shape))
1929 b = np.asarray(b)
1930 if b.ndim != 2 or b.shape[-1] != 3:
1931 raise ValueError("Expected input `b` to have shape (N, 3), "
1932 "got {}.".format(b.shape))
1934 if a.shape != b.shape:
1935 raise ValueError("Expected inputs `a` and `b` to have same shapes"
1936 ", got {} and {} respectively.".format(
1937 a.shape, b.shape))
1939 if weights is None:
1940 weights = np.ones(len(b))
1941 else:
1942 weights = np.asarray(weights)
1943 if weights.ndim != 1:
1944 raise ValueError("Expected `weights` to be 1 dimensional, got "
1945 "shape {}.".format(weights.shape))
1946 if weights.shape[0] != b.shape[0]:
1947 raise ValueError("Expected `weights` to have number of values "
1948 "equal to number of input vectors, got "
1949 "{} values and {} vectors.".format(
1950 weights.shape[0], b.shape[0]))
1952 B = np.einsum('ji,jk->ik', weights[:, None] * a, b)
1953 u, s, vh = np.linalg.svd(B)
1955 # Correct improper rotation if necessary (as in Kabsch algorithm)
1956 if np.linalg.det(u @ vh) < 0:
1957 s[-1] = -s[-1]
1958 u[:, -1] = -u[:, -1]
1960 C = np.dot(u, vh)
1962 if s[1] + s[2] < 1e-16 * s[0]:
1963 warnings.warn("Optimal rotation is not uniquely or poorly defined "
1964 "for the given sets of vectors.")
1966 rmsd = np.sqrt(max(
1967 np.sum(weights * np.sum(b ** 2 + a ** 2, axis=1)) - 2 * np.sum(s),
1968 0))
1970 if return_sensitivity:
1971 zeta = (s[0] + s[1]) * (s[1] + s[2]) * (s[2] + s[0])
1972 kappa = s[0] * s[1] + s[1] * s[2] + s[2] * s[0]
1973 with np.errstate(divide='ignore', invalid='ignore'):
1974 sensitivity = np.mean(weights) / zeta * (
1975 kappa * np.eye(3) + np.dot(B, B.T))
1976 return cls.from_matrix(C), rmsd, sensitivity
1977 else:
1978 return cls.from_matrix(C), rmsd
1981class Slerp(object):
1982 """Spherical Linear Interpolation of Rotations.
1984 The interpolation between consecutive rotations is performed as a rotation
1985 around a fixed axis with a constant angular velocity [1]_. This ensures
1986 that the interpolated rotations follow the shortest path between initial
1987 and final orientations.
1989 Parameters
1990 ----------
1991 times : array_like, shape (N,)
1992 Times of the known rotations. At least 2 times must be specified.
1993 rotations : `Rotation` instance
1994 Rotations to perform the interpolation between. Must contain N
1995 rotations.
1997 Methods
1998 -------
1999 __call__
2001 See Also
2002 --------
2003 Rotation
2005 Notes
2006 -----
2007 .. versionadded:: 1.2.0
2009 References
2010 ----------
2011 .. [1] https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp
2013 Examples
2014 --------
2015 >>> from scipy.spatial.transform import Rotation as R
2016 >>> from scipy.spatial.transform import Slerp
2018 Setup the fixed keyframe rotations and times:
2020 >>> key_rots = R.random(5, random_state=2342345)
2021 >>> key_times = [0, 1, 2, 3, 4]
2023 Create the interpolator object:
2025 >>> slerp = Slerp(key_times, key_rots)
2027 Interpolate the rotations at the given times:
2029 >>> times = [0, 0.5, 0.25, 1, 1.5, 2, 2.75, 3, 3.25, 3.60, 4]
2030 >>> interp_rots = slerp(times)
2032 The keyframe rotations expressed as Euler angles:
2034 >>> key_rots.as_euler('xyz', degrees=True)
2035 array([[ 14.31443779, -27.50095894, -3.7275787 ],
2036 [ -1.79924227, -24.69421529, 164.57701743],
2037 [146.15020772, 43.22849451, -31.34891088],
2038 [ 46.39959442, 11.62126073, -45.99719267],
2039 [-88.94647804, -49.64400082, -65.80546984]])
2041 The interpolated rotations expressed as Euler angles. These agree with the
2042 keyframe rotations at both endpoints of the range of keyframe times.
2044 >>> interp_rots.as_euler('xyz', degrees=True)
2045 array([[ 14.31443779, -27.50095894, -3.7275787 ],
2046 [ 4.74588574, -32.44683966, 81.25139984],
2047 [ 10.71094749, -31.56690154, 38.06896408],
2048 [ -1.79924227, -24.69421529, 164.57701743],
2049 [ 11.72796022, 51.64207311, -171.7374683 ],
2050 [ 146.15020772, 43.22849451, -31.34891088],
2051 [ 68.10921869, 20.67625074, -48.74886034],
2052 [ 46.39959442, 11.62126073, -45.99719267],
2053 [ 12.35552615, 4.21525086, -64.89288124],
2054 [ -30.08117143, -19.90769513, -78.98121326],
2055 [ -88.94647804, -49.64400082, -65.80546984]])
2057 """
2058 def __init__(self, times, rotations):
2059 if len(rotations) == 1:
2060 raise ValueError("`rotations` must contain at least 2 rotations.")
2062 times = np.asarray(times)
2063 if times.ndim != 1:
2064 raise ValueError("Expected times to be specified in a 1 "
2065 "dimensional array, got {} "
2066 "dimensions.".format(times.ndim))
2068 if times.shape[0] != len(rotations):
2069 raise ValueError("Expected number of rotations to be equal to "
2070 "number of timestamps given, got {} rotations "
2071 "and {} timestamps.".format(
2072 len(rotations), times.shape[0]))
2073 self.times = times
2074 self.timedelta = np.diff(times)
2076 if np.any(self.timedelta <= 0):
2077 raise ValueError("Times must be in strictly increasing order.")
2079 self.rotations = rotations[:-1]
2080 self.rotvecs = (self.rotations.inv() * rotations[1:]).as_rotvec()
2082 def __call__(self, times):
2083 """Interpolate rotations.
2085 Compute the interpolated rotations at the given `times`.
2087 Parameters
2088 ----------
2089 times : array_like
2090 Times to compute the interpolations at. Can be a scalar or
2091 1-dimensional.
2093 Returns
2094 -------
2095 interpolated_rotation : `Rotation` instance
2096 Object containing the rotations computed at given `times`.
2098 """
2099 # Clearly differentiate from self.times property
2100 compute_times = np.asarray(times)
2101 if compute_times.ndim > 1:
2102 raise ValueError("`times` must be at most 1-dimensional.")
2104 single_time = compute_times.ndim == 0
2105 compute_times = np.atleast_1d(compute_times)
2107 # side = 'left' (default) excludes t_min.
2108 ind = np.searchsorted(self.times, compute_times) - 1
2109 # Include t_min. Without this step, index for t_min equals -1
2110 ind[compute_times == self.times[0]] = 0
2111 if np.any(np.logical_or(ind < 0, ind > len(self.rotations) - 1)):
2112 raise ValueError("Interpolation times must be within the range "
2113 "[{}, {}], both inclusive.".format(
2114 self.times[0], self.times[-1]))
2116 alpha = (compute_times - self.times[ind]) / self.timedelta[ind]
2118 result = (self.rotations[ind] *
2119 Rotation.from_rotvec(self.rotvecs[ind] * alpha[:, None]))
2121 if single_time:
2122 result = result[0]
2124 return result