Блог

У меня есть два 2D облака точек с равным количеством элементов. Для этих элементов я знаю их соответствие, т. е. для каждой точки в PC1 я знаю соответствующий элемент в PC2 и наоборот.

Теперь я хотел бы оценить вращение между этими двумя облаками точек. То есть я хотел бы найти угол alpha , на который я должен повернуть все точки в PC1 вокруг начала координат так, чтобы расстояние между соответствующими точками в PC1 и PC2 было сведено к минимуму.

Я могу решить эту проблему с помощью линейного оптимизатора scipy (см. Ниже); однако эта оптимизация находится внутри цикла вдоль критического пути моего кода и является текущим узким местом.

 import numpy as np
from scipy.optimize import minimize_scalar
from math import sin, cos

# generate some data for demonstration purposes
# points in each point cloud are ordered by correspondence
num_points = 10

distance = np.random.rand(num_points) * 3
radii = np.random.rand(num_points) * 2*np.pi
pc1 = distance[:, None] * np.stack([np.cos(radii), np.sin(radii)], axis=1)

distance = np.random.rand(num_points) * 3
radii = np.random.rand(num_points) * 2*np.pi
pc2 = distance[:, None] * np.stack([np.cos(radii), np.sin(radii)], axis=1)

# solve using scipy
def score(alpha):
    rot_matrix = np.array([
        [cos(alpha), -sin(alpha)],
        [sin(alpha), cos(alpha)]
    ])
    pc1_rotated = (rot_matrix @ pc1.T).T

    sum_of_squares = np.sum((pc2 - pc1_rotated)**2, axis=1)
    mse = np.mean(sum_of_squares)

    return mse

# simple solution via scipy
result = minimize_scalar(
            score,
            bounds=(0, 2*np.pi),
            method="bounded",
            options={"maxiter": 1000},
        )

if result.success:
    print(f"Best angle: {result.x}")
else:
    raise RuntimeError(f"IK failed. Reason: {result.message}")

 

Существует ли более быстрое (потенциально аналитическое) решение этой проблемы?

Поскольку minimize_scalar используются только методы без производных, время выполнения оптимизации сильно зависит от времени, необходимого для оценки вашей целевой функции score . Следовательно, я бы рекомендовал максимально ускорить эту функцию.

Давайте определим время вашей функции и оптимизатора в качестве эталонного эталона:

 In [68]: %timeit score(0.5)
20.2 µs ± 203 ns per loop (mean ± std. dev. of 7 runs, 10000 loops each)
 

 In [69]: %timeit result = minimize_scalar(score,bounds=(0, 2*np.pi),method="bounded",options={"maxiter": 1000})
415 µs ± 7.27 µs per loop (mean ± std. dev. of 7 runs, 1000 loops each)
 

• Во-первых, обратите внимание , что (rot_matrix @ pc1.T).T это то же pc1 @ rot_matrix.T самое, что и, то есть нам нужно только транспонировать одну матрицу вместо двух.
• Далее, обратите -sin(alpha) = cos(alpha 5*pi/2) внимание, что sin(alpha) = cos(alpha 3*pi/2) и. Это означает, что нам нужен только один вызов функции np.cos для создания rot_matrix вместо четырех вызовов math.sin или math.cos .
• Наконец, вы можете вычислить mse более эффективно с помощью np.einsum .

Учитывая все моменты, функция может выглядеть следующим образом:

 k1 = 5*np.pi/2
k2 = 3*np.pi/2

def score2(alpha):
    rot_matrixT = np.cos((alpha, alpha k2, alpha   k1, alpha)).reshape(2,2)
    pc1_rotated = pc1 @ rot_matrixT
    diff = pc2 - pc1_rotated
    return np.einsum('ij,ij->', diff, diff) / num_points
 

Синхронизация функции снова дает

 In [70]: %timeit score(0.5)
9.26 µs ± 84.2 ns per loop (mean ± std. dev. of 7 runs, 100000 loops each)
 

и, следовательно, оптимизатор работает намного быстрее:

 In [71]: %timeit result = minimize_scalar(score, bounds=(0, 2*np.pi), method="bounded", options={"maxiter": 1000})
279 µs ± 1.79 µs per loop (mean ± std. dev. of 7 runs, 1000 loops each)
 

Если это все еще недостаточно быстро, вы можете точно в срок скомпилировать свою функцию с помощью Numba:

 In [60]: from numba import njit

In [61]: @njit
    ...: def score3(alpha):
    ...:     rot_matrix = np.array([
    ...:         [cos(alpha), -sin(alpha)],
    ...:         [sin(alpha), cos(alpha)]
    ...:     ])
    ...:     pc1_rotated = (rot_matrix @ pc1.T).T
    ...:     sum_of_squares = np.sum((pc2 - pc1_rotated)**2, axis=1)
    ...:     mse = np.mean(sum_of_squares)
    ...:     return mse
 

 In [62]: %timeit score3(0.5)
2.97 µs ± 47.8 ns per loop (mean ± std. dev. of 7 runs, 100000 loops each)
 

или перепишите его с помощью Cython. Просто для полноты картины, вот быстрая реализация Cython:

 In [45]: %%cython -c=-O3 -c=-march=native -c=-Wno-deprecated-declarations -c=-Wno-#warnings
    ...:
    ...: from libc.math cimport cos, sin
    ...: cimport numpy as np
    ...: import numpy as np
    ...: from cython cimport wraparound, boundscheck
    ...:
    ...: @wraparound(False)
    ...: @boundscheck(False)
    ...: cpdef double score4(double alpha, double[:, ::1] pc1, double[:, ::1] pc2):
    ...:     cdef int i
    ...:     cdef int N = pc1.shape[0]
    ...:     cdef double diff1 = 0.0
    ...:     cdef double diff2 = 0.0
    ...:     cdef double   mse = 0.0
    ...:     cdef double  rmT00 = cos(alpha)
    ...:     cdef double  rmT01 = sin(alpha)
    ...:     cdef double  rmT10 = -rmT01
    ...:     cdef double  rmT11 = rmT00
    ...:
    ...:     for i in range(N):
    ...:         diff1 = pc2[i,0] - (pc1[i,0]*rmT00   pc1[i,1]*rmT10)
    ...:         diff2 = pc2[i,1] - (pc1[i,0]*rmT01   pc1[i,1]*rmT11)
    ...:         mse   = diff1*diff1   diff2*diff2
    ...:     return mse / N
 

что дает

 In [48]: %timeit score4(0.5, pc1, pc2)
1.05 µs ± 15.8 ns per loop (mean ± std. dev. of 7 runs, 1000000 loops each)
 

И последнее, но не менее важное: вы можете записать необходимое условие первого порядка вашей проблемы и проверить, можно ли ее решить аналитически. В противном случае вы можете попытаться решить полученное нелинейное уравнение численно.