def dlog(v): r = v[0:3] dlogr = quat.dlog(r) MT = mt(v) return np.bmat([[dlogr, -dlogr*MT*dlogr],[np.zeros((3,3)),dlogr]])
def log(p): q = p.rot t = p.trans r = quat.log(q) D = quat.dlog(r) return np.vstack((r, D * t))