def difference(self,a,b):
w = se3.error(a,b)
return so3.cross_product(w[:3])+w[3:]
def difference(self, a, b):
w = se3.error(a, b)
return so3.cross_product(w[:3]) + w[3:]
def distance(self,a,b):
return vectorops.norm(se3.error((a[:9],a[9:]),(b[:9],b[9:])))
def distance(self, a, b):
return vectorops.norm(se3.error((a[:9], a[9:]), (b[:9], b[9:])))