align_vectors#
- classmethod Rotation.align_vectors(cls, a, b, weights=None, return_sensitivity=False)#
估计一个旋转，以最佳地对齐两组向量。
在坐标系 A 和 B 之间找到一个旋转，该旋转可以最佳地对齐在这些坐标系中观察到的一组向量 a 和 b。最小化以下损失函数以求解旋转矩阵 \(C\)
\[L(C) = \frac{1}{2} \sum_{i = 1}^{n} w_i \lVert \mathbf{a}_i - C \mathbf{b}_i \rVert^2 ,\]其中 \(w_i\) 是与每个向量对应的 权重。
使用 Kabsch 算法 [1] 估计旋转，并解决被称为“指向问题”或“Wahba 问题”的问题 [2]。
有两种特殊情况。第一种情况是，如果为 a 和 b 各给定一个向量，在这种情况下，将返回使 b 与 a 对齐的最短距离旋转。
第二种情况是其中一个权重为无穷大。在这种情况下，将如上所述计算主要无限权重向量之间的最短距离旋转。然后，计算围绕对齐的主向量旋转，以便根据上述损失函数最佳地对齐次向量。结果是这两个旋转的组合。此过程的结果与 Kabsch 算法相同，因为相应的权重在极限情况下接近无穷大。对于单个次向量，这被称为“对齐约束”算法 [3]。
对于这两种特殊情况（单个向量或无限权重），灵敏度矩阵都没有物理意义，如果请求它，将会引发错误。对于无限权重，主向量充当具有完美对齐的约束，因此即使它们的长度不同，它们对 rssd 的贡献也将被强制为 0。
- 参数：
- a类似数组，形状 (3,) 或 (N, 3)
在初始坐标系 A 中观察到的向量分量。 a 的每一行表示一个向量。
- b类似数组，形状 (3,) 或 (N, 3)
在另一个坐标系 B 中观察到的向量分量。 b 的每一行表示一个向量。
- weights类似数组，形状 (N,)，可选
描述向量观测值相对重要性的权重。如果为 None（默认值），则假设 weights 中的所有值均为 1。只有一个权重可以是无穷大，并且权重必须为正。
- return_sensitivity布尔值，可选
是否返回灵敏度矩阵。有关详细信息，请参阅注释。默认值为 False。
- 返回值：
- rotation
Rotation实例 将 b 变换为 a 的旋转的最佳估计。
- rssd浮点数
代表“均方根距离”。对齐后给定向量集之间平方距离的加权和的平方根。它等于
sqrt(2 * minimum_loss)，其中minimum_loss是针对找到的最佳旋转评估的损失函数。请注意，结果也将由向量的幅度加权，因此如果完美对齐的向量对的长度不同，则它们将具有非零 rssd。因此，根据用例，在调用此方法之前，可能需要将输入向量归一化为单位长度。- sensitivity_matrixndarray，形状 (3, 3)
估计旋转估计的灵敏度矩阵，如注释中所述。仅当 return_sensitivity 为 True 时才返回。如果对齐一对向量或存在无限权重，则无效，在这种情况下将引发错误。
- rotation
注释
灵敏度矩阵给出了估计旋转对向量测量值的小扰动的灵敏度。具体来说，我们将旋转估计误差视为坐标系 A 的一个小的旋转向量。假设 a 中的向量是在误差远小于其长度的情况下测量的，则灵敏度矩阵与该旋转向量的协方差成正比。为了获得真实的协方差矩阵，必须将返回的灵敏度矩阵乘以每次观测中方差的调和平均值 [4]。请注意， 权重 应与观测方差成反比，以获得一致的结果。例如，如果所有向量都以 0.01 的相同精度测量（权重 必须全部相等），则应将灵敏度矩阵乘以 0.01**2 以获得协方差。
有关协方差估计的更严格的讨论，请参阅 [5]。有关指向问题和最小正确指向的更多讨论，请参阅 [6]。
参考文献
[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.
示例
>>> import numpy as np >>> from scipy.spatial.transform import Rotation as R
在这里，我们运行基线 Kabsch 算法以最佳地对齐两组向量，其中
b集的最后两次向量测量值存在噪声>>> 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.]])
当我们将旋转应用于
b时，我们得到接近于a的向量>>> rot.apply(b) array([[0. , 1. , 0. ], [0. , 1. , 1.1], [0. , 1. , 0.9]])
第一个向量的误差为 0，最后两个向量的误差幅度为 0.1。 rssd 是加权平方误差之和的平方根，默认权重均为 1，因此在这种情况下， rssd 的计算公式为
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
此示例的灵敏度矩阵如下所示
>>> sens array([[0.2, 0. , 0.], [0. , 1.5, 1.], [0. , 1. , 1.]])
特殊情况 1：找到单个向量之间的最小旋转
>>> 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.])
特殊情况 2：一个无限权重。在这里，我们找到了可以精确对齐的主向量和次向量之间的旋转
>>> 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.]])
在这里，次向量必须是最佳拟合的
>>> 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.]])