scipy.spatial.transform.Rotation.align_vectors#

Rotation.align_vectors(type 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].

Parameters:
aarray_like, shape (N, 3)

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

barray_like, shape (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.

return_sensitivitybool, optional

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

Returns:
estimated_rotationRotation instance

Best estimate of the rotation that transforms b to a.

rssdfloat

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.

sensitivity_matrixndarray, shape (3, 3)

Sensitivity matrix of the estimated rotation estimate as explained in Notes. Returned only when return_sensitivity is True.

Notes

This method can also compute 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 [3] 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 [2] for more rigorous discussion of the covariance estimation.

References

[2]

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.