Example #1
0
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]])
Example #2
0
def log(p):
	q = p.rot
	t = p.trans
	r = quat.log(q)
	D = quat.dlog(r)
	return np.vstack((r, D * t))