Эффективные кватернионы в преобразование Эйлера

Question

Я использую следующую функцию Python для преобразования кватернионов в углы Эйлера:

import math

def quaternion_to_euler_angle(w, x, y, z):
    ysqr = y * y

    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    X = math.degrees(math.atan2(t0, t1))

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y = math.degrees(math.asin(t2))

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    Z = math.degrees(math.atan2(t3, t4))

    return X, Y, Z

Я хотел бы преобразовать Pandas DataFrame, который имеет столбцы "w", "quat_x", "quat_y" и "quat_z", в углы Эйлера. В настоящее время я перебираю каждую строку DataFrame, используя цикл for, и вызываю функцию quaternion_to_euler_angle() в каждой строке. Это очень медленно, потому что у меня более 400 000 строк.

Есть ли более эффективный способ сделать это? Например, я мог бы передать DataFrame (или отдельный ряд) в quaternion_to_euler_angle(), но тогда проблема состоит в том, чтобы изменить quaternion_to_euler_angle(), чтобы он мог обрабатывать DataFrames вместо целых чисел.

Answer 1

Мы могли бы использовать векторизацию NumPy ufuncs вместо их math аналоговых модулей, которые работают со всеми данными массива и при этом имеют минимальное изменение (я) -

import numpy as np

def quaternion_to_euler_angle_vectorized1(w, x, y, z):
    ysqr = y * y

    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    X = np.degrees(np.arctan2(t0, t1))

    t2 = +2.0 * (w * y - z * x)
    t2 = np.where(t2>+1.0,+1.0,t2)
    #t2 = +1.0 if t2 > +1.0 else t2

    t2 = np.where(t2<-1.0, -1.0, t2)
    #t2 = -1.0 if t2 < -1.0 else t2
    Y = np.degrees(np.arcsin(t2))

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    Z = np.degrees(np.arctan2(t3, t4))

    return X, Y, Z 

Итак,единственными заменами были:

math.degrees <-> np.degrees
math.atan2   <-> np.arctan2
math.asin    <-> np.arcsin

и np.where для векторизованных проверок и назначений.

Следовательно, мы получаем наше векторизованное решение, например, так: *

# For df.columns = ['w', 'quat_x', 'quat_y', 'quat_z']
X,Y,Z = quaternion_to_euler_angle_vectorized1(*df.values.T)

# If needed as a dataframe output 
df_out = pd.DataFrame({'X':X,'Y':Y,'Z':Z})

Времяна 400,000 строках -

In [55]: np.random.seed(0)
    ...: a = np.random.rand(400000,4)
    ...: df = pd.DataFrame(a)
    ...: df.columns = ["w", "quat_x", "quat_y" , "quat_z"]

In [56]: %timeit quaternion_to_euler_angle_vectorized1(*df.values.T)
1 loops, best of 3: 70.6 ms per loop

Оптимизация # 1

Используйте np.clip для замены double np.where -

def quaternion_to_euler_angle_vectorized2(w, x, y, z):
    ysqr = y * y

    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    X = np.degrees(np.arctan2(t0, t1))

    t2 = +2.0 * (w * y - z * x)

    t2 = np.clip(t2, a_min=-1.0, a_max=1.0)
    Y = np.degrees(np.arcsin(t2))

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    Z = np.degrees(np.arctan2(t3, t4))

    return X, Y, Z

Сроки по тем же данным -

In [70]: %timeit quaternion_to_euler_angle_vectorized2(*df.values.T)
10 loops, best of 3: 65.2 ms per loop