估計兩個二維點雲之間的旋轉

Question

我有兩個元素數量相同的二維點雲。 對於這些元素，我知道它們的對應關系，即對於 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}")

這個問題有更快的（潛在的分析）解決方案嗎？

Answer 1

由於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的四個調用。

• 最后，您可以通過np.einsum更有效地計算mse 。

考慮到所有要點，該函數可能如下所示：

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)

最后但並非最不重要的一點是，您可以寫下問題的一階必要條件並檢查它是否可以解析解決。 否則，您可以嘗試數值求解所產生的非線性方程。