scipy.spatial.transform.Rotation.

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。

参数:

aarray_like, 形状 (3,) 或 (N, 3)

在初始帧A中观察到的向量分量。a 的每一行表示一个向量。

barray_like, 形状 (3,) 或 (N, 3)

在另一个参考系B中观察到的矢量分量。b 的每一行表示一个矢量。

权重类似数组的形状 (N,)，可选

描述向量观测相对重要性的权重。如果为 None（默认），则假定 weights 中的所有值均为 1。有且仅有一个权重可以是无穷大，且权重必须为正。

return_sensitivitybool, 可选

是否返回敏感度矩阵。详见注释。默认值为 False。

返回:

旋转 : Rotation 实例旋转实例

将 b 转换为 a 的最佳旋转估计。

rssd浮动

代表“根和平方距离”。在对齐后给定向量集之间的平方距离的加权和的平方根。它等于 sqrt(2 * minimum_loss)，其中 minimum_loss 是针对找到的最佳旋转评估的损失函数。请注意，结果还将根据向量的大小进行加权，因此如果向量对的长度不同，即使它们完全对齐，rssd 也将不为零。因此，根据使用情况，可能需要在调用此方法之前将输入向量归一化为单位长度。

sensitivity_matrixndarray, 形状 (3, 3)

如注释中所解释的估计旋转估计的灵敏度矩阵。仅当 return_sensitivity 为 True 时返回。如果对齐单对向量或存在无限权重，则无效，在这种情况下将引发错误。

注释

灵敏度矩阵给出了估计旋转对向量测量的小扰动的灵敏度。具体来说，我们将旋转估计误差视为帧A的小旋转向量。假设向量`a`的测量误差显著小于其长度，灵敏度矩阵与该旋转向量的协方差成正比。要得到真实的协方差矩阵，返回的灵敏度矩阵必须乘以每次观测中方差的调和平均数 [4]。请注意，weights 应与观测方差成反比，以获得一致的结果。例如，如果所有向量都以0.01的相同精度测量（`weights`必须全部相等），那么你应该将灵敏度矩阵乘以0.01**2以得到协方差。

关于协方差估计的更严格讨论，请参阅 [5]。关于指向问题和最小适当指向的更多讨论，请参阅 [6]。

参考文献

[1]

https://en.wikipedia.org/wiki/Kabsch_algorithm

[2]

https://en.wikipedia.org/wiki/Wahba%27s_problem

[3]

Magner, Robert, “通过轨迹和动量设定点优化扩展目标跟踪能力。” 小型卫星会议, 2018.

[4]

https://zh.wikipedia.org/wiki/调和平均数

[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, 和 Leiba Rodman, “在真实欧几里得空间中指向”, 《制导、控制与动力学杂志》, 第20卷, 第5期, 1997年, 第916-922页。

示例

Try it in your browser!

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

Go BackOpen In Tab