Python SciPy Rotation.align_vectors用法及代碼示例

本文簡要介紹 python 語言中 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] 估計的，並解決了所謂的 “pointing problem” 或“Wahba 問題”。

有兩種特殊情況。第一種情況是，如果為 a 和 b 給出單個向量，則返回將 b 與 a 對齊的最短距離旋轉。

第二種情況是其中一個權重為無窮大時。在這種情況下，初級無限權重向量之間的最短距離旋轉的計算如上。然後，計算圍繞對齊的主向量的旋轉，以便根據上述損失函數最佳地對齊次向量。結果就是這兩個旋轉的組合。此過程的結果與 Kabsch 算法相同，相應的權重在極限下接近無窮大。對於單個輔助向量，這稱為align-constrain 算法 [2]。

對於這兩種特殊情況(單個向量或無限權重)，靈敏度矩陣沒有物理意義，如果需要，將會引發錯誤。對於無限權重，主向量充當完美對齊的約束，因此它們對 rssd 的貢獻將被迫為 0。

參數 ：：

a： 數組，形狀 (3,) 或 (N, 3)

在初始幀 A 中觀察到的向量分量。a 的每一行表示一個向量。

b： 數組，形狀 (3,) 或 (N, 3)

在另一幀 B 中觀察到的向量分量。b 的每一行表示一個向量。

weights： 數組 形狀 (N,)，可選

說明矢量觀測值相對重要性的權重。如果無(默認)，則權重中的所有值都假定為 1。一個且隻有一個權重可以是無窮大，並且權重必須為正。

return_sensitivity： 布爾型，可選

是否返回靈敏度矩陣。詳細信息請參見注釋。默認值為 False。

返回 ：：

rotation： Rotation 實例

將 b 轉換為 a 的旋轉的最佳估計。

rssd： 浮點數

代表“距離和平方根”。對齊後給定向量集之間的平方距離的加權和的平方根。它等於sqrt(2 * minimum_loss)，其中minimum_loss是針對找到的最佳旋轉評估的損失函數。請注意，結果還將按向量的大小進行加權，因此完美對齊的向量對將具有非零值RSSD如果它們的長度不同。因此，根據使用情況，可能需要在調用此方法之前將輸入向量標準化為單位長度。

sensitivity_matrix： ndarray，形狀 (3, 3)

估計旋轉估計的靈敏度矩陣，如注釋中所述。僅當 return_sensitivity 為 True 時返回。如果對齊一對向量或權重無窮大則無效，在這種情況下會引發錯誤。

注意：

該方法還可以計算估計旋轉對矢量測量的小擾動的敏感性。具體來說，我們將旋轉估計誤差視為幀 A 的一個小旋轉向量。靈敏度矩陣與該旋轉向量的協方差成正比，假設以下向量a測量誤差明顯小於其長度。為了得到真正的協方差矩陣，返回的靈敏度矩陣必須乘以調和平均值[3]每個觀察值的方差。注意權重應該與觀察方差成反比以獲得一致的結果。例如，如果所有向量均以 0.01 的相同精度進行測量(權重必須全部相等)，那麽您應該將靈敏度矩陣乘以 0.01**2 以獲得協方差。

有關協方差估計的更嚴格討論，請參閱[4]。有關指向問題和最小正確指向的更多討論，請參閱[5]。

參考：

[1]

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

[2]

羅伯特·馬格納，“通過軌跡和動量設定點優化擴展目標跟蹤能力。”小衛星會議，2018。

[3]

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

[4]

F. Landis Markley，“使用矢量觀測的姿態確定：快速最優矩陣算法”，《宇航科學雜誌》，卷。 41，第2期，1993年，第261-280頁。

[5]

Bar-Itzhack、Itzhack Y.、Daniel Hershkowitz 和 Leiba Rodman，“指向真實歐幾裏得空間”，《製導、控製和動力學雜誌》，卷。 20，第 5 期，1997 年，第 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.]])

這裏的輔助向量必須是best-fit：

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