quaternion.py 文件源码

def dexp(v):
    x = np.linalg.norm(v)
    a = trig.sinox(x)
    b = trig.cosox2(x)
    c = trig.sinox3(x)
    I = np.identity(3)
    S = skew(v)
    W = v * v.transpose()
    return a*I + b*S + c*W