估计两个二维点云之间的旋转

Question

I have two 2D point clouds with an equal number of elements. For these elements I know their correspondence, ie for each point in PC1 I know the corresponding element in PC2 and vice versa.

I would now like to estimate the rotation between these two point clouds. That is, I would like to find the angle alpha by which I must rotate all points in PC1 around the origin such that the distance between corresponding points in PC1 and PC2 is minimized.

I can solve this using scipy's linear optimizer (see below); however, this optimization sits inside a loop along the critical path of my code and is the current bottleneck.

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}")

Is there a faster (potentially analytic) solution to this problem?

Answer 1

Since minimize_scalar only uses derivative-free methods, the optimization runtime depends heavily on the time needed to evaluate your objective function score . Consequently, I'd recommend accelerating this function as much as possible.

Let's time your function and the optimizer as benchmark reference:

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)

• Firstly, note that (rot_matrix @ pc1.T).T is the same as pc1 @ rot_matrix.T , ie we only need to transpose one matrix instead of two.

• Next, note that -sin(alpha) = cos(alpha + 5*pi/2) and sin(alpha) = cos(alpha + 3*pi/2) . This means that we only need one function call of np.cos to create the rot_matrix instead of four calls of math.sin or math.cos .

• Lastly, you can compute the mse more efficiently by np.einsum .

Considering all points, the function can look like this:

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

Timing the function again yields

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

and therefore, the optimizer is much faster:

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)

If that still is not fast enough, you can just-in-time compile your function by 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)

or rewrite it using Cython . Just for the sake of completeness, here's a fast Cython implementation:

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

which yields

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)

Last but not least, you can write down the first-order necessary condition of your problem and check whether it can be solved analytically. Otherwise, you can try to solve the resulting nonlinear equation numerically.