Hide keyboard shortcuts

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 

7 

8 

9_AXIS_TO_IND = {'x': 0, 'y': 1, 'z': 2} 

10 

11 

12def _elementary_basis_vector(axis): 

13 b = np.zeros(3) 

14 b[_AXIS_TO_IND[axis]] = 1 

15 return b 

16 

17 

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 

26 

27 if extrinsic: 

28 seq = seq[::-1] 

29 

30 if matrix.ndim == 2: 

31 matrix = matrix[None, :, :] 

32 num_rotations = matrix.shape[0] 

33 

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]) 

39 

40 # Step 2 

41 sl = np.dot(np.cross(n1, n2), n3) 

42 cl = np.dot(n1, n3) 

43 

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)) 

48 

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)) 

57 

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]) 

66 

67 # Steps 5, 6 

68 eps = 1e-7 

69 safe1 = (np.abs(angles[:, 1]) >= eps) 

70 safe2 = (np.abs(angles[:, 1] - np.pi) >= eps) 

71 

72 # Step 4 (Completion) 

73 angles[:, 1] += offset 

74 

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]) 

81 

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]) 

111 

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) 

120 

121 # Dont adjust gimbal locked angle sequences 

122 adjust_mask = np.logical_and(adjust_mask, safe_mask) 

123 

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 

127 

128 angles[angles < -np.pi] += 2 * np.pi 

129 angles[angles > np.pi] -= 2 * np.pi 

130 

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.") 

135 

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 

141 

142 

143def _make_elementary_quat(axis, angles): 

144 quat = np.zeros((angles.shape[0], 4)) 

145 

146 quat[:, 3] = np.cos(angles / 2) 

147 quat[:, _AXIS_TO_IND[axis]] = np.sin(angles / 2) 

148 return quat 

149 

150 

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 

157 

158 

159def _elementary_quat_compose(seq, angles, intrinsic=False): 

160 result = _make_elementary_quat(seq[0], angles[:, 0]) 

161 

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 

172 

173 

174class Rotation(object): 

175 """Rotation in 3 dimensions. 

176 

177 This class provides an interface to initialize from and represent rotations 

178 with: 

179 

180 - Quaternions 

181 - Rotation Matrices 

182 - Rotation Vectors 

183 - Euler Angles 

184 

185 The following operations on rotations are supported: 

186 

187 - Application on vectors 

188 - Rotation Composition 

189 - Rotation Inversion 

190 - Rotation Indexing 

191 

192 Indexing within a rotation is supported since multiple rotation transforms 

193 can be stored within a single `Rotation` instance. 

194 

195 To create `Rotation` objects use ``from_...`` methods (see examples below). 

196 ``Rotation(...)`` is not supposed to be instantiated directly. 

197 

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 

220 

221 See Also 

222 -------- 

223 Slerp 

224 

225 Notes 

226 ----- 

227 .. versionadded: 1.2.0 

228 

229 Examples 

230 -------- 

231 >>> from scipy.spatial.transform import Rotation as R 

232 

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. 

236 

237 Consider a counter-clockwise rotation of 90 degrees about the z-axis. This 

238 corresponds to the following quaternion (in scalar-last format): 

239 

240 >>> r = R.from_quat([0, 0, np.sin(np.pi/4), np.cos(np.pi/4)]) 

241 

242 The rotation can be expressed in any of the other formats: 

243 

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.]) 

252 

253 The same rotation can be initialized using a rotation matrix: 

254 

255 >>> r = R.from_matrix([[0, -1, 0], 

256 ... [1, 0, 0], 

257 ... [0, 0, 1]]) 

258 

259 Representation in other formats: 

260 

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.]) 

267 

268 The rotation vector corresponding to this rotation is given by: 

269 

270 >>> r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1])) 

271 

272 Representation in other formats: 

273 

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.]) 

282 

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: 

285 

286 >>> r = R.from_euler('z', 90, degrees=True) 

287 

288 Again, the object is representation independent and can be converted to any 

289 other format: 

290 

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]) 

299 

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: 

303 

304 >>> r = R.from_euler('zyx', [ 

305 ... [90, 0, 0], 

306 ... [0, 45, 0], 

307 ... [45, 60, 30]], degrees=True) 

308 

309 The other representations also now return a stack of 3 rotations. For 

310 example: 

311 

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]]) 

316 

317 Applying the above rotations onto a vector: 

318 

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]]) 

324 

325 A `Rotation` instance can be indexed and sliced as if it were a single 

326 1D array or list: 

327 

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]]) 

341 

342 Multiple rotations can be composed using the ``*`` operator: 

343 

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]) 

352 

353 Finally, it is also possible to invert rotations: 

354 

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.]]) 

360 

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. 

364 

365 """ 

366 def __init__(self, quat, normalize=True, copy=True): 

367 self._single = False 

368 quat = np.asarray(quat, dtype=float) 

369 

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)) 

373 

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 

380 

381 if normalize: 

382 self._quat = quat.copy() 

383 norms = scipy.linalg.norm(quat, axis=1) 

384 

385 zero_norms = norms == 0 

386 if zero_norms.any(): 

387 raise ValueError("Found zero norm quaternions in `quat`.") 

388 

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 

393 

394 def __len__(self): 

395 """Number of rotations contained in this object. 

396 

397 Multiple rotations can be stored in a single instance. 

398 

399 Returns 

400 ------- 

401 length : int 

402 Number of rotations stored in object. 

403 

404 """ 

405 return self._quat.shape[0] 

406 

407 @classmethod 

408 def from_quat(cls, quat, normalized=None): 

409 """Initialize from quaternions. 

410 

411 3D rotations can be represented using unit-norm quaternions [1]_. 

412 

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. 

422 

423 .. deprecated:: 1.4.0 

424 

425 Returns 

426 ------- 

427 rotation : `Rotation` instance 

428 Object containing the rotations represented by input quaternions. 

429 

430 References 

431 ---------- 

432 .. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 

433 

434 Examples 

435 -------- 

436 >>> from scipy.spatial.transform import Rotation as R 

437 

438 Initialize a single rotation: 

439 

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,) 

445 

446 Initialize multiple rotations in a single object: 

447 

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) 

457 

458 It is also possible to have a stack of a single rotation: 

459 

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) 

465 

466 Quaternions are normalized before initialization. 

467 

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) 

476 

477 return cls(quat, normalize=True) 

478 

479 @classmethod 

480 def from_matrix(cls, matrix): 

481 """Initialize from rotation matrix. 

482 

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]_. 

486 

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. 

492 

493 Returns 

494 ------- 

495 rotation : `Rotation` instance 

496 Object containing the rotations represented by the rotation 

497 matrices. 

498 

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. 

505 

506 Examples 

507 -------- 

508 >>> from scipy.spatial.transform import Rotation as R 

509 

510 Initialize a single rotation: 

511 

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) 

518 

519 Initialize multiple rotations in a single object: 

520 

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) 

534 

535 If input matrices are not special orthogonal (orthogonal with 

536 determinant equal to +1), then a special orthogonal estimate is stored: 

537 

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 

552 

553 It is also possible to have a stack containing a single rotation: 

554 

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) 

565 

566 Notes 

567 ----- 

568 This function was called from_dcm before. 

569 

570 .. versionadded:: 1.4.0 

571 """ 

572 is_single = False 

573 matrix = np.asarray(matrix, dtype=float) 

574 

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)) 

578 

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 

585 

586 num_rotations = matrix.shape[0] 

587 

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) 

592 

593 quat = np.empty((num_rotations, 4)) 

594 

595 ind = np.nonzero(choices != 3)[0] 

596 i = choices[ind] 

597 j = (i + 1) % 3 

598 k = (j + 1) % 3 

599 

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] 

604 

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] 

610 

611 quat /= np.linalg.norm(quat, axis=1)[:, None] 

612 

613 if is_single: 

614 return cls(quat[0], normalize=False, copy=False) 

615 else: 

616 return cls(quat, normalize=False, copy=False) 

617 

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) 

623 

624 @classmethod 

625 def from_rotvec(cls, rotvec): 

626 """Initialize from rotation vectors. 

627 

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]_. 

631 

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. 

637 

638 Returns 

639 ------- 

640 rotation : `Rotation` instance 

641 Object containing the rotations represented by input rotation 

642 vectors. 

643 

644 References 

645 ---------- 

646 .. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector 

647 

648 Examples 

649 -------- 

650 >>> from scipy.spatial.transform import Rotation as R 

651 

652 Initialize a single rotation: 

653 

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,) 

659 

660 Initialize multiple rotations in one object: 

661 

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) 

670 

671 It is also possible to have a stack of a single rotaton: 

672 

673 >>> r = R.from_rotvec([[0, 0, np.pi/2]]) 

674 >>> r.as_rotvec().shape 

675 (1, 3) 

676 

677 """ 

678 is_single = False 

679 rotvec = np.asarray(rotvec, dtype=float) 

680 

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)) 

684 

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 

691 

692 num_rotations = rotvec.shape[0] 

693 

694 norms = np.linalg.norm(rotvec, axis=1) 

695 small_angle = (norms <= 1e-3) 

696 large_angle = ~small_angle 

697 

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]) 

703 

704 quat = np.empty((num_rotations, 4)) 

705 quat[:, :3] = scale[:, None] * rotvec 

706 quat[:, 3] = np.cos(norms / 2) 

707 

708 if is_single: 

709 return cls(quat[0], normalize=False, copy=False) 

710 else: 

711 return cls(quat, normalize=False, copy=False) 

712 

713 @classmethod 

714 def from_euler(cls, seq, angles, degrees=False): 

715 """Initialize from Euler angles. 

716 

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. 

721 

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]_. 

725 

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: 

737 

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 

743 

744 For 2- and 3-character wide `seq`, `angles` can be: 

745 

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 

751 

752 degrees : bool, optional 

753 If True, then the given angles are assumed to be in degrees. 

754 Default is False. 

755 

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. 

761 

762 References 

763 ---------- 

764 .. [1] https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations 

765 

766 Examples 

767 -------- 

768 >>> from scipy.spatial.transform import Rotation as R 

769 

770 Initialize a single rotation along a single axis: 

771 

772 >>> r = R.from_euler('x', 90, degrees=True) 

773 >>> r.as_quat().shape 

774 (4,) 

775 

776 Initialize a single rotation with a given axis sequence: 

777 

778 >>> r = R.from_euler('zyx', [90, 45, 30], degrees=True) 

779 >>> r.as_quat().shape 

780 (4,) 

781 

782 Initialize a stack with a single rotation around a single axis: 

783 

784 >>> r = R.from_euler('x', [90], degrees=True) 

785 >>> r.as_quat().shape 

786 (1, 4) 

787 

788 Initialize a stack with a single rotation with an axis sequence: 

789 

790 >>> r = R.from_euler('zyx', [[90, 45, 30]], degrees=True) 

791 >>> r.as_quat().shape 

792 (1, 4) 

793 

794 Initialize multiple elementary rotations in one object: 

795 

796 >>> r = R.from_euler('x', [90, 45, 30], degrees=True) 

797 >>> r.as_quat().shape 

798 (3, 4) 

799 

800 Initialize multiple rotations in one object: 

801 

802 >>> r = R.from_euler('zyx', [[90, 45, 30], [35, 45, 90]], degrees=True) 

803 >>> r.as_quat().shape 

804 (2, 4) 

805 

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)) 

811 

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)) 

817 

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)) 

821 

822 seq = seq.lower() 

823 

824 angles = np.asarray(angles, dtype=float) 

825 if degrees: 

826 angles = np.deg2rad(angles) 

827 

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)) 

851 

852 if angles.ndim == 1: 

853 # (1, num_axes) 

854 angles = angles[None, :] 

855 is_single = True 

856 

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)) 

862 

863 quat = _elementary_quat_compose(seq, angles, intrinsic) 

864 return cls(quat[0] if is_single else quat, normalize=False, copy=False) 

865 

866 def as_quat(self): 

867 """Represent as quaternions. 

868 

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. 

874 

875 Returns 

876 ------- 

877 quat : `numpy.ndarray`, shape (4,) or (N, 4) 

878 Shape depends on shape of inputs used for initialization. 

879 

880 References 

881 ---------- 

882 .. [1] https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 

883 

884 Examples 

885 -------- 

886 >>> from scipy.spatial.transform import Rotation as R 

887 

888 Represent a single rotation: 

889 

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,) 

897 

898 Represent a stack with a single rotation: 

899 

900 >>> r = R.from_quat([[0, 0, 0, 1]]) 

901 >>> r.as_quat().shape 

902 (1, 4) 

903 

904 Represent multiple rotations in a single object: 

905 

906 >>> r = R.from_rotvec([[np.pi, 0, 0], [0, 0, np.pi/2]]) 

907 >>> r.as_quat().shape 

908 (2, 4) 

909 

910 """ 

911 if self._single: 

912 return self._quat[0].copy() 

913 else: 

914 return self._quat.copy() 

915 

916 def as_matrix(self): 

917 """Represent as rotation matrix. 

918 

919 3D rotations can be represented using rotation matrices, which 

920 are 3 x 3 real orthogonal matrices with determinant equal to +1 [1]_. 

921 

922 Returns 

923 ------- 

924 matrix : ndarray, shape (3, 3) or (N, 3, 3) 

925 Shape depends on shape of inputs used for initialization. 

926 

927 References 

928 ---------- 

929 .. [1] https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions 

930 

931 Examples 

932 -------- 

933 >>> from scipy.spatial.transform import Rotation as R 

934 

935 Represent a single rotation: 

936 

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) 

944 

945 Represent a stack with a single rotation: 

946 

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) 

954 

955 Represent multiple rotations: 

956 

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) 

967 

968 Notes 

969 ----- 

970 This function was called as_dcm before. 

971 

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] 

978 

979 x2 = x * x 

980 y2 = y * y 

981 z2 = z * z 

982 w2 = w * w 

983 

984 xy = x * y 

985 zw = z * w 

986 xz = x * z 

987 yw = y * w 

988 yz = y * z 

989 xw = x * w 

990 

991 num_rotations = len(self) 

992 matrix = np.empty((num_rotations, 3, 3)) 

993 

994 matrix[:, 0, 0] = x2 - y2 - z2 + w2 

995 matrix[:, 1, 0] = 2 * (xy + zw) 

996 matrix[:, 2, 0] = 2 * (xz - yw) 

997 

998 matrix[:, 0, 1] = 2 * (xy - zw) 

999 matrix[:, 1, 1] = - x2 + y2 - z2 + w2 

1000 matrix[:, 2, 1] = 2 * (yz + xw) 

1001 

1002 matrix[:, 0, 2] = 2 * (xz + yw) 

1003 matrix[:, 1, 2] = 2 * (yz - xw) 

1004 matrix[:, 2, 2] = - x2 - y2 + z2 + w2 

1005 

1006 if self._single: 

1007 return matrix[0] 

1008 else: 

1009 return matrix 

1010 

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() 

1015 

1016 def as_rotvec(self): 

1017 """Represent as rotation vectors. 

1018 

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]_. 

1022 

1023 Returns 

1024 ------- 

1025 rotvec : ndarray, shape (3,) or (N, 3) 

1026 Shape depends on shape of inputs used for initialization. 

1027 

1028 References 

1029 ---------- 

1030 .. [1] https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector 

1031 

1032 Examples 

1033 -------- 

1034 >>> from scipy.spatial.transform import Rotation as R 

1035 

1036 Represent a single rotation: 

1037 

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,) 

1043 

1044 Represent a stack with a single rotation: 

1045 

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) 

1051 

1052 Represent multiple rotations in a single object: 

1053 

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) 

1060 

1061 """ 

1062 quat = self._quat.copy() 

1063 # w > 0 to ensure 0 <= angle <= pi 

1064 quat[quat[:, 3] < 0] *= -1 

1065 

1066 angle = 2 * np.arctan2(np.linalg.norm(quat[:, :3], axis=1), quat[:, 3]) 

1067 

1068 small_angle = (angle <= 1e-3) 

1069 large_angle = ~small_angle 

1070 

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)) 

1077 

1078 rotvec = scale[:, None] * quat[:, :3] 

1079 

1080 if self._single: 

1081 return rotvec[0] 

1082 else: 

1083 return rotvec 

1084 

1085 def as_euler(self, seq, degrees=False): 

1086 """Represent as Euler angles. 

1087 

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]_. 

1091 

1092 The algorithm from [2]_ has been used to calculate Euler angles for the 

1093 rotation about a given sequence of axes. 

1094 

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. 

1100 

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. 

1112 

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: 

1118 

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: 

1122 

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) 

1126 

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 

1134 

1135 Examples 

1136 -------- 

1137 >>> from scipy.spatial.transform import Rotation as R 

1138 

1139 Represent a single rotation: 

1140 

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,) 

1146 

1147 Represent a stack of single rotation: 

1148 

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) 

1154 

1155 Represent multiple rotations in a single object: 

1156 

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) 

1167 

1168 """ 

1169 if len(seq) != 3: 

1170 raise ValueError("Expected 3 axes, got {}.".format(seq)) 

1171 

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)) 

1178 

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)) 

1182 

1183 seq = seq.lower() 

1184 

1185 angles = _compute_euler_from_matrix(self.as_matrix(), seq, extrinsic) 

1186 if degrees: 

1187 angles = np.rad2deg(angles) 

1188 

1189 return angles[0] if self._single else angles 

1190 

1191 def apply(self, vectors, inverse=False): 

1192 """Apply this rotation to a set of vectors. 

1193 

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: 

1196 

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. 

1202 

1203 In terms of rotation matricies, this application is the same as 

1204 ``self.as_matrix().dot(vectors)``. 

1205 

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. 

1217 

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: 

1223 

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. 

1229 

1230 Examples 

1231 -------- 

1232 >>> from scipy.spatial.transform import Rotation as R 

1233 

1234 Single rotation applied on a single vector: 

1235 

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,) 

1246 

1247 Single rotation applied on multiple vectors: 

1248 

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) 

1262 

1263 Multiple rotations on a single vector: 

1264 

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) 

1279 

1280 Multiple rotations on multiple vectors. Each rotation is applied on the 

1281 corresponding vector: 

1282 

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) 

1294 

1295 It is also possible to apply the inverse rotation: 

1296 

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 ]]) 

1306 

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)) 

1312 

1313 single_vector = False 

1314 if vectors.shape == (3,): 

1315 single_vector = True 

1316 vectors = vectors[None, :] 

1317 

1318 matrix = self.as_matrix() 

1319 if self._single: 

1320 matrix = matrix[None, :, :] 

1321 

1322 n_vectors = vectors.shape[0] 

1323 n_rotations = len(self) 

1324 

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)) 

1330 

1331 if inverse: 

1332 result = np.einsum('ikj,ik->ij', matrix, vectors) 

1333 else: 

1334 result = np.einsum('ijk,ik->ij', matrix, vectors) 

1335 

1336 if self._single and single_vector: 

1337 return result[0] 

1338 else: 

1339 return result 

1340 

1341 def __mul__(self, other): 

1342 """Compose this rotation with the other. 

1343 

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())``. 

1348 

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``. 

1355 

1356 Returns 

1357 ------- 

1358 composition : `Rotation` instance 

1359 This function supports composition of multiple rotations at a time. 

1360 The following cases are possible: 

1361 

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. 

1368 

1369 Examples 

1370 -------- 

1371 >>> from scipy.spatial.transform import Rotation as R 

1372 

1373 Composition of two single rotations: 

1374 

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.]]) 

1390 

1391 Composition of two objects containing equal number of rotations: 

1392 

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]]) 

1405 

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) 

1417 

1418 def inv(self): 

1419 """Invert this rotation. 

1420 

1421 Composition of a rotation with its inverse results in an identity 

1422 transformation. 

1423 

1424 Returns 

1425 ------- 

1426 inverse : `Rotation` instance 

1427 Object containing inverse of the rotations in the current instance. 

1428 

1429 Examples 

1430 -------- 

1431 >>> from scipy.spatial.transform import Rotation as R 

1432 

1433 Inverting a single rotation: 

1434 

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.]) 

1439 

1440 Inverting multiple rotations: 

1441 

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. ]]) 

1447 

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) 

1454 

1455 def magnitude(self): 

1456 """Get the magnitude(s) of the rotation(s). 

1457 

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. 

1463 

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. ]) 

1470 

1471 Magnitude of a single rotation: 

1472 

1473 >>> r[0].magnitude() 

1474 3.141592653589793 

1475 """ 

1476 

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) 

1481 

1482 if self._single: 

1483 return angles[0] 

1484 else: 

1485 return angles 

1486 

1487 def mean(self, weights=None): 

1488 """Get the mean of the rotations. 

1489 

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. 

1496 

1497 Returns 

1498 ------- 

1499 mean : `Rotation` instance 

1500 Object containing the mean of the rotations in the current 

1501 instance. 

1502 

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()``. 

1509 

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.") 

1534 

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) 

1538 

1539 def reduce(self, left=None, right=None, return_indices=False): 

1540 """Reduce this rotation with the provided rotation groups. 

1541 

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. 

1546 

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. 

1550 

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. 

1562 

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() 

1580 

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 

1585 

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. 

1591 

1592 def split_rotation(R): 

1593 q = np.atleast_2d(R.as_quat()) 

1594 return q[:, -1], q[:, :-1] 

1595 

1596 p = self 

1597 ps, pv = split_rotation(p) 

1598 ls, lv = split_rotation(left) 

1599 rs, rv = split_rotation(right) 

1600 

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)) 

1607 

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) 

1612 

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] 

1619 

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 

1628 

1629 @classmethod 

1630 def create_group(cls, group, axis='Z'): 

1631 """Create a 3D rotation group. 

1632 

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: 

1638 

1639 * I: Icosahedral group 

1640 * O: Octahedral group 

1641 * T: Tetrahedral group 

1642 * D: Dicyclic group 

1643 * C: Cyclic group 

1644 

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'. 

1648 

1649 Returns 

1650 ------- 

1651 rotation : `Rotation` instance 

1652 Object containing the elements of the rotation group. 

1653 

1654 Notes 

1655 ----- 

1656 This method generates rotation groups only. The full 3-dimensional 

1657 point groups [PointGroups]_ also contain reflections. 

1658 

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) 

1666 

1667 def __getitem__(self, indexer): 

1668 """Extract rotation(s) at given index(es) from object. 

1669 

1670 Create a new `Rotation` instance containing a subset of rotations 

1671 stored in this object. 

1672 

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. 

1678 

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. 

1686 

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. ]]) 

1698 

1699 Indexing using a single index: 

1700 

1701 >>> p = r[0] 

1702 >>> p.as_quat() 

1703 array([0.70710678, 0.70710678, 0. , 0. ]) 

1704 

1705 Array slicing: 

1706 

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. ]]) 

1711 

1712 """ 

1713 return self.__class__(self._quat[indexer], normalize=False) 

1714 

1715 @classmethod 

1716 def identity(cls, num=None): 

1717 """Get identity rotation(s). 

1718 

1719 Composition with the identity rotation has no effect. 

1720 

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. 

1726 

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) 

1738 

1739 @classmethod 

1740 def random(cls, num=None, random_state=None): 

1741 """Generate uniformly distributed rotations. 

1742 

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. 

1752 

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. 

1758 

1759 Examples 

1760 -------- 

1761 >>> from scipy.spatial.transform import Rotation as R 

1762 

1763 Sample a single rotation: 

1764 

1765 >>> R.random(random_state=1234).as_euler('zxy', degrees=True) 

1766 array([-110.5976185 , 55.32758512, 76.3289269 ]) 

1767 

1768 Sample a stack of rotations: 

1769 

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]]) 

1776 

1777 """ 

1778 random_state = check_random_state(random_state) 

1779 

1780 if num is None: 

1781 sample = random_state.normal(size=4) 

1782 else: 

1783 sample = random_state.normal(size=(num, 4)) 

1784 

1785 return cls(sample) 

1786 

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)) 

1801 

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)) 

1806 

1807 if b.shape[0] == 1: 

1808 raise ValueError("Rotation cannot be estimated using a single " 

1809 "vector.") 

1810 

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) 

1824 

1825 if not normalized: 

1826 a = a / scipy.linalg.norm(a, axis=1)[:, None] 

1827 b = b / scipy.linalg.norm(b, axis=1)[:, None] 

1828 

1829 B = np.einsum('ji,jk->ik', weights[:, None] * a, b) 

1830 u, s, vh = np.linalg.svd(B) 

1831 

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] 

1836 

1837 C = np.dot(u, vh) 

1838 

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.") 

1844 

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 

1849 

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. 

1853 

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`: 

1858 

1859 .. math:: 

1860 

1861 L(C) = \\frac{1}{2} \\sum_{i = 1}^{n} w_i \\lVert \\mathbf{a}_i - 

1862 C \\mathbf{b}_i \\rVert^2 , 

1863 

1864 where :math:`w_i`'s are the `weights` corresponding to each vector. 

1865 

1866 The rotation is estimated with Kabsch algorithm [1]_. 

1867 

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. 

1883 

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. 

1896 

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. 

1912 

1913 Refer to [2]_ for more rigorous discussion of the covariance 

1914 estimation. 

1915 

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)) 

1933 

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)) 

1938 

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])) 

1951 

1952 B = np.einsum('ji,jk->ik', weights[:, None] * a, b) 

1953 u, s, vh = np.linalg.svd(B) 

1954 

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] 

1959 

1960 C = np.dot(u, vh) 

1961 

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.") 

1965 

1966 rmsd = np.sqrt(max( 

1967 np.sum(weights * np.sum(b ** 2 + a ** 2, axis=1)) - 2 * np.sum(s), 

1968 0)) 

1969 

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 

1979 

1980 

1981class Slerp(object): 

1982 """Spherical Linear Interpolation of Rotations. 

1983 

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. 

1988 

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. 

1996 

1997 Methods 

1998 ------- 

1999 __call__ 

2000 

2001 See Also 

2002 -------- 

2003 Rotation 

2004 

2005 Notes 

2006 ----- 

2007 .. versionadded:: 1.2.0 

2008 

2009 References 

2010 ---------- 

2011 .. [1] https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp 

2012 

2013 Examples 

2014 -------- 

2015 >>> from scipy.spatial.transform import Rotation as R 

2016 >>> from scipy.spatial.transform import Slerp 

2017 

2018 Setup the fixed keyframe rotations and times: 

2019 

2020 >>> key_rots = R.random(5, random_state=2342345) 

2021 >>> key_times = [0, 1, 2, 3, 4] 

2022 

2023 Create the interpolator object: 

2024 

2025 >>> slerp = Slerp(key_times, key_rots) 

2026 

2027 Interpolate the rotations at the given times: 

2028 

2029 >>> times = [0, 0.5, 0.25, 1, 1.5, 2, 2.75, 3, 3.25, 3.60, 4] 

2030 >>> interp_rots = slerp(times) 

2031 

2032 The keyframe rotations expressed as Euler angles: 

2033 

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]]) 

2040 

2041 The interpolated rotations expressed as Euler angles. These agree with the 

2042 keyframe rotations at both endpoints of the range of keyframe times. 

2043 

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]]) 

2056 

2057 """ 

2058 def __init__(self, times, rotations): 

2059 if len(rotations) == 1: 

2060 raise ValueError("`rotations` must contain at least 2 rotations.") 

2061 

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)) 

2067 

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) 

2075 

2076 if np.any(self.timedelta <= 0): 

2077 raise ValueError("Times must be in strictly increasing order.") 

2078 

2079 self.rotations = rotations[:-1] 

2080 self.rotvecs = (self.rotations.inv() * rotations[1:]).as_rotvec() 

2081 

2082 def __call__(self, times): 

2083 """Interpolate rotations. 

2084 

2085 Compute the interpolated rotations at the given `times`. 

2086 

2087 Parameters 

2088 ---------- 

2089 times : array_like 

2090 Times to compute the interpolations at. Can be a scalar or 

2091 1-dimensional. 

2092 

2093 Returns 

2094 ------- 

2095 interpolated_rotation : `Rotation` instance 

2096 Object containing the rotations computed at given `times`. 

2097 

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.") 

2103 

2104 single_time = compute_times.ndim == 0 

2105 compute_times = np.atleast_1d(compute_times) 

2106 

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])) 

2115 

2116 alpha = (compute_times - self.times[ind]) / self.timedelta[ind] 

2117 

2118 result = (self.rotations[ind] * 

2119 Rotation.from_rotvec(self.rotvecs[ind] * alpha[:, None])) 

2120 

2121 if single_time: 

2122 result = result[0] 

2123 

2124 return result