pose.py 文件源码

def mt(v):
    r = v[0:3]
    t = v[3:6]
    x = np.linalg.norm(r)
    b = trig.cosox2(x)
    c = trig.sinox3(x)
    g = trig.specialFun1(x)
    h = trig.specialFun3(x)
    I = np.identity(3)
    return b*quat.skew(t) + c*(r*t.transpose() + t*r.transpose()) + v3.dot(r,t)*((c - b) * I + g*quat.skew(r) + h*r*r.transpose())