scipy.spatial.transform.Rotation.

align_vectors#

метод класса Вращение.align_vectors(cls, a, b, веса=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\)’s являются веса соответствующий каждому вектору.

Вращение оценивается с помощью алгоритма Кабша [1]и решает так называемую "задачу наведения" или "задачу Вахбы" [2].

Существует два особых случая. Первый — если задан единичный вектор для a и b, в котором кратчайшее расстояние вращения, выравнивающее b to a возвращается.

Второй случай — когда один из весов равен бесконечности. В этом случае вычисляется кратчайшее вращение расстояния между первичными бесконечно весовыми векторами, как описано выше. Затем вычисляется вращение вокруг выровненных первичных векторов так, чтобы вторичные векторы были оптимально выровнены согласно вышеуказанной функции потерь. Результат — композиция этих двух вращений. Результат этого процесса совпадает с алгоритмом Кабша, когда соответствующий вес стремится к бесконечности в пределе. Для одного вторичного вектора это известно как алгоритм «выровнять-ограничить». [3].

Для обоих особых случаев (одиночные векторы или бесконечный вес) матрица чувствительности не имеет физического смысла, и будет вызвана ошибка, если она запрошена. Для бесконечного веса основные векторы действуют как ограничение с идеальным выравниванием, поэтому их вклад в rssd будут принудительно установлены в 0, даже если они имеют разную длину.

Параметры:

aarray_like, форма (3,) или (N, 3): Компоненты вектора, наблюдаемые в начальной системе отсчёта A. Каждая строка a обозначает вектор.
barray_like, форма (3,) или (N, 3): Векторные компоненты, наблюдаемые в другой системе отсчета B. Каждая строка b обозначает вектор.
весаarray_like, форма (N,), опционально: Веса, описывающие относительную важность векторных наблюдений. Если None (по умолчанию), то все значения в веса предполагаются равными 1. Один и только один вес может быть бесконечностью, и веса должны быть положительными.
return_sensitivitybool, необязательно: Возвращать ли матрицу чувствительности. Подробности см. в примечаниях. По умолчанию False.

Возвращает:

вращениеRotation экземпляр: Лучшая оценка вращения, которое преобразует b to a.
rssdfloat: Обозначает 'корень суммы квадратов расстояний'. Квадратный корень из взвешенной суммы квадратов расстояний между заданными наборами векторов после выравнивания. Равен sqrt(2 * minimum_loss), где minimum_loss — это функция потерь, вычисленная для найденного оптимального вращения. Обратите внимание, что результат также будет взвешен по величинам векторов, поэтому идеально выровненные пары векторов будут иметь ненулевое значение rssd если они не одинаковой длины. Поэтому, в зависимости от конкретного случая использования, может быть желательно нормализовать входные векторы до единичной длины перед вызовом этого метода.
sensitivity_matrixndarray, форма (3, 3): Матрица чувствительности оцененного вращения, как объяснено в примечаниях. Возвращается только когда return_sensitivity равно True. Недействительно при выравнивании одной пары векторов или при наличии бесконечного веса, в этих случаях будет вызвана ошибка.

Примечания

Матрица чувствительности дает чувствительность оцененного вращения к малым возмущениям векторных измерений. В частности, мы рассматриваем ошибку оценки вращения как малый вектор вращения кадра A. Матрица чувствительности пропорциональна ковариации этого вектора вращения при условии, что векторы в a были измерены с ошибками, значительно меньшими их длин. Чтобы получить истинную ковариационную матрицу, возвращаемая матрица чувствительности должна быть умножена на гармоническое среднее [4] дисперсии в каждом наблюдении. Обратите внимание, что веса должны быть обратно пропорциональны дисперсиям наблюдений для получения согласованных результатов. Например, если все векторы измерены с одинаковой точностью 0.01 (веса должны быть все равны), тогда следует умножить матрицу чувствительности на 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, “Extending target tracking capabilities through trajectory and momentum setpoint optimization.” Small Satellite Conference, 2018.

[4]

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

[5]

Ф. Лэндис Маркли, "Определение ориентации с использованием векторных наблюдений: быстрый оптимальный матричный алгоритм", Journal of Astronautical Sciences, Том 41, №2, 1993, стр. 261-280.

[6]

Бар-Ицхак, Ицхак Й., Даниэль Хершковиц и Лейба Родман, «Наведение в реальном евклидовом пространстве», 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

Здесь мы запускаем базовый алгоритм Кабша для наилучшего выравнивания двух наборов векторов, где присутствует шум на последних двух измерениях векторов в 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.]])

Когда мы применяем вращение к 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.]])