class Transform(object):
def __init__(self, *args):
if len(args) == 0:
self.translation = np.array([0.0, 0.0, 0.0])
self.quaternion = Quaternion()
if len(args) == 7:
self.translation = np.array(args[0:3])
self.quaternion = Quaternion(args[3:7])
if len(args) == 2:
self.quaternion = Quaternion(args[0])
self.translation = np.array(args[1])
if len(args) == 1:
if isinstance(args[0], Transform):
self.quaternion = args[0].quaternion
self.translation = args[0].translation
elif isinstance(args[0], (list, tuple)):
self.translation = np.array(args[0][0:3])
self.quaternion = Quaternion(args[0][3:7])
self.quaternion.normalize()
def __mul__(self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform(other.translation) + self.translation
return Transform(rot, trans)
def inverse(self):
rot = self.quaternion.conjugate()
trans = -self.quaternion.conjugate().transform(self.translation)
return Transform(rot, trans)
def transform(self, v):
res = self.quaternion.transform(v) + self.translation
return res
def __str__(self):
return "quaternion: %s,\ntranslation: %s" % (self.quaternion,
self.translation)
def __getitem__(self, i):
if i < 3:
return self.translation[i]
if i < 7:
return self.quaternion.toTuple()[i - 3]
raise IndexError("index out of range")
def __len__(self):
return 7
def toHomogeneousMatrix(self):
H = np.eye(4)
H[:3, :3] = self.quaternion.toRotationMatrix()
H[:3, 3] = self.translation
return H
def toTuple(self):
return tuple(self.translation) + self.quaternion.toTuple()
class Transform (object):
def __init__ (self, *args):
if len (args) == 0:
self.translation = np.array ([0.,0.,0.])
self.quaternion = Quaternion ()
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__ (self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform (other.translation) +\
self.translation
return Transform (rot, trans)
def inverse (self):
rot = self.quaternion.conjugate()
trans = - self.quaternion.conjugate().transform (self.translation)
return Transform (rot, trans)
def transform (self, v):
res = self.quaternion.transform (v) + self.translation
return res
def __str__ (self):
return "quaternion: %s,\ntranslation: %s"%(self.quaternion,
self.translation)
def __getitem__ (self, i):
if i<3: return self.translation [i]
if i<7: return self.quaternion.toTuple () [i-3]
raise IndexError ("index out of range")
def __len__ (self):
return 7
def toHomogeneousMatrix (self):
H = np.eye(4)
H[:3,:3] = self.quaternion.toRotationMatrix()
H[:3,3] = self.translation
return H
def toTuple (self):
return tuple (self.translation) + self.quaternion.toTuple ()
class Transform (object):
def __init__ (self, *args):
if len (args) == 7:
self.translation = np.array (args [0:3])
self.quaternion = Quaternion (args [3:7])
if len (args) == 2:
self.quaternion = Quaternion (args [0])
self.translation = np.array (args [1])
if len (args) == 1:
if isinstance (args [0], Transform) :
self.quaternion = args [0].quaternion
self.translation = args [0].translation
elif isinstance (args [0], list):
self.translation = np.array (args [0][0:3])
self.quaternion = Quaternion (args [0][3:7])
self.quaternion.normalize ()
def __mul__ (self, other):
rot = self.quaternion * other.quaternion
trans = self.quaternion.transform (other.translation) +\
self.translation
return Transform (rot, trans)
def inverse (self):
rot = self.quaternion.conjugate()
trans = - self.quaternion.conjugate().transform (self.translation)
return Transform (rot, trans)
def __str__ (self):
return "quaternion: %s,\ntranslation: %s"%(self.quaternion,
self.translation)
raise RuntimeError(
"Error in sub-quaternions, line {0}".format(iline))
# test multiplication
with open('./test/div-quaternions.csv', 'r') as f:
r = csv.reader(f, delimiter=',')
iline = 0
for line in r:
iline += 1
data = list(map(float, line))
q1 = Quaternion(data[0:4])
q2 = Quaternion(data[4:8])
q3 = Quaternion(data[8:12])
if abs((q1 / q2) - q3) > 1e-10:
raise RuntimeError(
"Error in div-quaternions, line {0}".format(iline))
# test transformation
with open('./test/transform-quaternions.csv', 'r') as f:
r = csv.reader(f, delimiter=',')
iline = 0
for line in r:
iline += 1
data = list(map(float, line))
q = Quaternion(data[0:4])
u = np.array(data[4:7])
v = np.array(data[7:10])
if np.linalg.norm(q.transform(u) - v) > 1e-10:
raise RuntimeError \
("Error in transform-quaternions, line {0}".format (iline))
raise RuntimeError ("Error in sub-quaternions, line {0}".format
(iline))
# test multiplication
with open ('./test/div-quaternions.csv', 'r') as f:
r = csv.reader (f, delimiter=',')
iline=0
for line in r:
iline+=1
data = map (float, line)
q1 = Quaternion (data [0:4])
q2 = Quaternion (data [4:8])
q3 = Quaternion (data [8:12])
if abs((q1/q2)-q3) > 1e-10:
raise RuntimeError ("Error in div-quaternions, line {0}".format
(iline))
# test transformation
with open ('./test/transform-quaternions.csv', 'r') as f:
r = csv.reader (f, delimiter=',')
iline=0
for line in r:
iline+=1
data = map (float, line)
q = Quaternion (data [0:4])
u = np.array (data [4:7])
v = np.array (data [7:10])
if np.linalg.norm (q.transform (u) - v) > 1e-10:
raise RuntimeError \
("Error in transform-quaternions, line {0}".format (iline))
