Ctrl+K

scipy.spatial.transform.Rotation.align_vectors#

classmethod Rotation.align_vectors(cls, a, b, weights=None, return_sensitivity=False)#

Estimate a rotation to optimally align two sets of vectors.

Find a rotation between frames A and B which best aligns a set of vectors a and b observed in these frames. The following loss function is minimized to solve for the rotation matrix \(C\):

\[L(C) = \frac{1}{2} \sum_{i = 1}^{n} w_i \lVert \mathbf{a}_i - C \mathbf{b}_i \rVert^2 ,\]

where \(w_i\)’s are the weights corresponding to each vector.

The rotation is estimated with Kabsch algorithm [1], and solves what is known as the “pointing problem”, or “Wahba’s problem” [2].

There are two special cases. The first is if a single vector is given for a and b, in which the shortest distance rotation that aligns b to a is returned.

The second is when one of the weights is infinity. In this case, the shortest distance rotation between the primary infinite weight vectors is calculated as above. Then, the rotation about the aligned primary vectors is calculated such that the secondary vectors are optimally aligned per the above loss function. The result is the composition of these two rotations. The result via this process is the same as the Kabsch algorithm as the corresponding weight approaches infinity in the limit. For a single secondary vector this is known as the “align-constrain” algorithm [3].

For both special cases (single vectors or an infinite weight), the sensitivity matrix does not have physical meaning and an error will be raised if it is requested. For an infinite weight, the primary vectors act as a constraint with perfect alignment, so their contribution to rssd will be forced to 0 even if they are of different lengths.

Parameters:
aarray_like, shape (3,) or (N, 3)

Vector components observed in initial frame A. Each row of a denotes a vector.

barray_like, shape (3,) or (N, 3)

Vector components observed in another frame B. Each row of b denotes a vector.

weightsarray_like shape (N,), optional

Weights describing the relative importance of the vector observations. If None (default), then all values in weights are assumed to be 1. One and only one weight may be infinity, and weights must be positive.

return_sensitivitybool, optional

Whether to return the sensitivity matrix. See Notes for details. Default is False.

Returns:
rotationRotation instance

Best estimate of the rotation that transforms b to a.

rssdfloat

Stands for “root sum squared distance”. Square root of the weighted sum of the squared distances between the given sets of vectors after alignment. It is equal to sqrt(2 * minimum_loss), where minimum_loss is the loss function evaluated for the found optimal rotation. Note that the result will also be weighted by the vectors’ magnitudes, so perfectly aligned vector pairs will have nonzero rssd if they are not of the same length. So, depending on the use case it may be desirable to normalize the input vectors to unit length before calling this method.

sensitivity_matrixndarray, shape (3, 3)

Sensitivity matrix of the estimated rotation estimate as explained in Notes. Returned only when return_sensitivity is True. Not valid if aligning a single pair of vectors or if there is an infinite weight, in which cases an error will be raised.

Notes

The sensitivity matrix gives the sensitivity of the estimated rotation to small perturbations of the vector measurements. Specifically we consider the rotation estimate error as a small rotation vector of frame A. The sensitivity matrix is proportional to the covariance of this rotation vector assuming that the vectors in a was measured with errors significantly less than their lengths. To get the true covariance matrix, the returned sensitivity matrix must be multiplied by harmonic mean [4] of variance in each observation. Note that weights are supposed to be inversely proportional to the observation variances to get consistent results. For example, if all vectors are measured with the same accuracy of 0.01 (weights must be all equal), then you should multiple the sensitivity matrix by 0.01**2 to get the covariance.

Refer to [5] for more rigorous discussion of the covariance estimation. See [6] for more discussion of the pointing problem and minimal proper pointing.

References

[3]

Magner, Robert, “Extending target tracking capabilities through trajectory and momentum setpoint optimization.” Small Satellite Conference, 2018.

[5]

F. Landis Markley, “Attitude determination using vector observations: a fast optimal matrix algorithm”, Journal of Astronautical Sciences, Vol. 41, No.2, 1993, pp. 261-280.

[6]

Bar-Itzhack, Itzhack Y., Daniel Hershkowitz, and Leiba Rodman, “Pointing in Real Euclidean Space”, Journal of Guidance, Control, and Dynamics, Vol. 20, No. 5, 1997, pp. 916-922.

Examples

>>> import numpy as np
>>> from scipy.spatial.transform import Rotation as R

Here we run the baseline Kabsch algorithm to best align two sets of vectors, where there is noise on the last two vector measurements of the b set:

>>> a = [[0, 1, 0], [0, 1, 1], [0, 1, 1]]
>>> b = [[1, 0, 0], [1, 1.1, 0], [1, 0.9, 0]]
>>> rot, rssd, sens = R.align_vectors(a, b, return_sensitivity=True)
>>> rot.as_matrix()
array([[0., 0., 1.],
       [1., 0., 0.],
       [0., 1., 0.]])

When we apply the rotation to b, we get vectors close to a:

>>> rot.apply(b)
array([[0. , 1. , 0. ],
       [0. , 1. , 1.1],
       [0. , 1. , 0.9]])

The error for the first vector is 0, and for the last two the error is magnitude 0.1. The rssd is the square root of the sum of the weighted squared errors, and the default weights are all 1, so in this case the rssd is calculated as sqrt(1 * 0**2 + 1 * 0.1**2 + 1 * (-0.1)**2) = 0.141421356237308

>>> a - rot.apply(b)
array([[ 0., 0.,  0. ],
       [ 0., 0., -0.1],
       [ 0., 0.,  0.1]])
>>> np.sqrt(np.sum(np.ones(3) @ (a - rot.apply(b))**2))
0.141421356237308
>>> rssd
0.141421356237308

The sensitivity matrix for this example is as follows:

>>> sens
array([[0.2, 0. , 0.],
       [0. , 1.5, 1.],
       [0. , 1. , 1.]])

Special case 1: Find a minimum rotation between single vectors:

>>> a = [1, 0, 0]
>>> b = [0, 1, 0]
>>> rot, _ = R.align_vectors(a, b)
>>> rot.as_matrix()
array([[0., 1., 0.],
       [-1., 0., 0.],
       [0., 0., 1.]])
>>> rot.apply(b)
array([1., 0., 0.])

Special case 2: One infinite weight. Here we find a rotation between primary and secondary vectors that can align exactly:

>>> a = [[0, 1, 0], [0, 1, 1]]
>>> b = [[1, 0, 0], [1, 1, 0]]
>>> rot, _ = R.align_vectors(a, b, weights=[np.inf, 1])
>>> rot.as_matrix()
array([[0., 0., 1.],
       [1., 0., 0.],
       [0., 1., 0.]])
>>> rot.apply(b)
array([[0., 1., 0.],
       [0., 1., 1.]])

Here the secondary vectors must be best-fit:

>>> a = [[0, 1, 0], [0, 1, 1]]
>>> b = [[1, 0, 0], [1, 2, 0]]
>>> rot, _ = R.align_vectors(a, b, weights=[np.inf, 1])
>>> rot.as_matrix()
array([[0., 0., 1.],
       [1., 0., 0.],
       [0., 1., 0.]])
>>> rot.apply(b)
array([[0., 1., 0.],
       [0., 1., 2.]])
On this page