def drawParticles2d(self, V, frame_id='origin', name='particles', lname='yaw', c=['b'], lc=['r'], ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d,N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2,:], np.zeros((1,N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d,l*N))
DT = np.zeros((d,l*N))
for v in V:
tt = np.zeros((d,N))
for i in range(0,N):
t = Pose.from_rigid_transform(0,RigidTransform(xyzw=Quaternion.from_rpy(0,0,v[2,i]).to_xyzw(), tvec=[v[0,i], v[1,i], 0]))
pert = Pose.from_rigid_transform(0,RigidTransform(tvec=[ll,0,0]))
tv = (t.oplus(pert)).tvec
tt[0:2,i] = tv[0:2]
count +=1
DT[0:d,(count*N):((count+1)*N)] = tt
Dv = np.vstack((v[0:2,:], np.zeros((1,N))))
DV[0:d,(count*N):((count+1)*N)] = Dv
publish_line_segments(lname, DV.T, DT.T, c=lc[0], frame_id=frame_id)
def random(cls, t=1):
q_wxyz = [rnd.random(), rnd.random(), rnd.random(), rnd.random()]
qmag = np.sqrt(sum([x * x for x in q_wxyz]))
q_wxyz = [x / qmag for x in q_wxyz]
translation = [
rnd.uniform(-t, t),
rnd.uniform(-t, t),
rnd.uniform(-t, t)
]
return cls(Quaternion.from_wxyz(q_wxyz), translation)
def drawParticles2d(self,
V,
frame_id='origin',
name='particles',
lname='yaw',
c=['b'],
lc=['r'],
ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d, N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2, :], np.zeros((1, N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d, l * N))
DT = np.zeros((d, l * N))
for v in V:
tt = np.zeros((d, N))
for i in range(0, N):
t = Pose.from_rigid_transform(
0,
RigidTransform(xyzw=Quaternion.from_rpy(
0, 0, v[2, i]).to_xyzw(),
tvec=[v[0, i], v[1, i], 0]))
pert = Pose.from_rigid_transform(
0, RigidTransform(tvec=[ll, 0, 0]))
tv = (t.oplus(pert)).tvec
tt[0:2, i] = tv[0:2]
count += 1
DT[0:d, (count * N):((count + 1) * N)] = tt
Dv = np.vstack((v[0:2, :], np.zeros((1, N))))
DV[0:d, (count * N):((count + 1) * N)] = Dv
publish_line_segments(lname,
DV.T,
DT.T,
c=lc[0],
frame_id=frame_id)
def from_matrix(cls, T):
return cls(Quaternion.from_matrix(T), T[:3, 3])
def __init__(self, xyzw=[0., 0., 0., 1.], tvec=[0., 0., 0.]):
""" Initialize a RigidTransform with Quaternion and 3D Position """
self.real = Quaternion(xyzw)
self.dual = (Quaternion(xyzw=[tvec[0], tvec[1], tvec[2], 0.]) *
self.real) * 0.5
class DualQuaternion(object):
"""
SE(3) rigid transform class that allows compounding of 6-DOF poses
and provides common transformations that are commonly seen in geometric problems.
"""
def __init__(self, xyzw=[0., 0., 0., 1.], tvec=[0., 0., 0.]):
""" Initialize a RigidTransform with Quaternion and 3D Position """
self.real = Quaternion(xyzw)
self.dual = (Quaternion(xyzw=[tvec[0], tvec[1], tvec[2], 0.]) *
self.real) * 0.5
def __repr__(self):
return 'real: %s dual: %s' % \
(np.array_str(self.real, precision=2, suppress_small=True),
np.array_str(self.dual, precision=2, suppress_small=True))
def __add__(self, other):
return DualQuaternion.from_dq(self.real + other.real,
self.dual + other.dual)
def __mul__(self, other):
"""
Left-multiply RigidTransform with another rigid transform
Two variants:
RigidTransform: Identical to oplus operation
ndarray: transform [N x 3] point set (X_2 = p_21 * X_1)
"""
if isinstance(other, DualQuaternion):
return DualQuaternion.from_dq(
other.real * self.real,
other.dual * self.real + other.real * self.dual)
elif isinstance(other, float):
return DualQuaternion.from_dq(self.real * other, self.dual * other)
# elif isinstance(other, nd.array):
# X = np.hstack([other, np.ones((len(other),1))]).T
# return (np.dot(self.matrix, X).T)[:,:3]
else:
raise TypeError('__mul__ typeerror {:}'.format(type(other)))
def __rmul__(self, other):
raise NotImplementedError('Right multiply not implemented yet!')
def normalize(self):
""" Check validity of unit-quaternion norm """
self.real.normalize()
self.dual.noramlize()
def dot(self, other):
return self.real.dot(other.real)
def inverse(self):
""" Returns a new RigidTransform that corresponds to the inverse of this one """
qinv = self.dual.inverse()
return DualQuaternion.from_dq(qinv.q, qinv.rotate(-self.tvec))
def conjugate(self):
return DualQuaternion.from_dq(self.real.conjugate(),
self.dual.conjugate())
def to_matrix(self):
""" Returns a 4x4 homogenous matrix of the form [R t; 0 1] """
result = self.rotation.to_matrix()
result[:3, 3] = self.translation
return result
def to_Rt(self):
""" Returns rotation R, and translational vector t """
T = self.to_matrix()
return T[:3, :3].copy(), T[:3, 3].copy()
def to_wxyz(self):
return self.rotation.to_wxyz()
def to_xyzw(self):
return self.rotation.to_xyzw()
@classmethod
def from_dq(cls, r, d):
if not isinstance(r, DualQuaternion):
raise TypeError('r is not DualQuaternion')
if not not isinstance(d, DualQuaternion):
raise TypeError('d is not DualQuaternion')
a = cls()
a.real = r
a.dual = d
return a
@classmethod
def from_Rt(cls, R, t):
T = np.eye(4)
T[:3, :3] = R.copy()
T[:3, 3] = t.copy()
return cls.from_matrix(T)
@classmethod
def from_matrix(cls, T):
return cls(Quaternion.from_matrix(T), T[:3, 3])
@property
def rotation(self):
return self.real
@property
def translation(self):
t = self.dual.q * self.real.conjugate()
return np.array([t.x, t.y, t.z]) * 2.0
@classmethod
def identity(cls):
return cls()
def from_matrix(cls, T):
sR_t = np.eye(4)
sR_t[:3, :3] = T[:3, :3] / T[3, 3]
return cls(Quaternion.from_matrix(sR_t), T[:3, 3], scale=1.0 / T[3, 3])
def from_angle_axis(cls, angle, axis, tvec):
return cls(Quaternion.from_angle_axis(angle, axis), tvec)
def from_Rt(cls, R, t):
T = np.eye(4)
T[:3, :3] = R.copy()
return cls(Quaternion.from_matrix(T), t)
def from_rpyxyz(cls, roll, pitch, yaw, x, y, z, axes='rxyz'):
q = Quaternion.from_rpy(roll, pitch, yaw, axes=axes)
return cls(q, (x, y, z))
def __init__(self, xyzw=[0., 0., 0., 1.], tvec=[0., 0., 0.]):
""" Initialize a RigidTransform with Quaternion and 3D Position """
self.quat = Quaternion(xyzw)
self.tvec = np.float32(tvec)
class RigidTransform(object):
"""
SE(3) rigid transform class that allows compounding of 6-DOF poses
and provides common transformations that are commonly seen in geometric problems.
quat: Quaternion/Rotation (xyzw)
tvec: Translation (xyz)
"""
def __init__(self, xyzw=[0., 0., 0., 1.], tvec=[0., 0., 0.]):
""" Initialize a RigidTransform with Quaternion and 3D Position """
self.quat = Quaternion(xyzw)
self.tvec = np.float32(tvec)
def __repr__(self):
return 'rpy (rxyz): %s tvec: %s' % \
(np.array_str(self.quat.to_rpy(axes='rxyz'),
precision=2, suppress_small=True),
np.array_str(self.tvec, precision=2, suppress_small=True))
def __mul__(self, other):
"""
Left-multiply RigidTransform with another rigid transform
Two variants:
RigidTransform: Identical to oplus operation
- self * other = self.oplus(other)
ndarray: transform [N x 3] point set (X_2 = p_21 * X_1)
"""
if isinstance(other, RigidTransform):
return self.oplus(other)
else:
X = np.hstack([other, np.ones((len(other), 1))]).T
return (np.dot(self.matrix, X).T)[:, :3]
def __rmul__(self, other):
raise NotImplementedError('Right multiply not implemented yet!')
def copy(self):
return RigidTransform(self.quat.copy(), self.tvec.copy())
def inverse(self):
"""
Returns a new RigidTransform that corresponds to the
inverse of this one
"""
qinv = self.quat.inverse()
return RigidTransform(qinv, qinv.rotate(-self.tvec))
def oplus(self, other):
if isinstance(other, RigidTransform):
t = self.quat.rotate(other.tvec) + self.tvec
r = self.quat * other.quat
return RigidTransform(r, t)
elif isinstance(other, list):
return [self.oplus(o) for o in other]
else:
raise TypeError("Type inconsistent", type(other), other.__class__)
def ominus(self, other):
raise NotImplementedError()
# if not isinstance(other, RigidTransform):
# raise TypeError("Type inconsistent")
# oinv = other.inverse()
# return oinv.oplus(self)
def wrt(self, p_tr):
"""
self: p_ro, desired: p_to
o: other, r: reference, t: target
"""
return self * p_tr
def rotate_vec(self, v):
if v.ndim == 2:
return np.vstack([self.quat.rotate(v_) for v_ in v])
else:
assert (v.ndim == 1 or (v.ndim == 2 and v.shape[0] == 1))
return self.quat.rotate(v)
def interpolate(self, other, w):
"""
LERP interpolation on rotation, and linear interpolation on position
Other approaches:
https://www.cvl.isy.liu.se/education/graduate/geometry-for-computer-vision-2014/geometry2014/lecture7.pdf
"""
assert (w >= 0 and w <= 1.0)
return self.from_Rt(
self.rotation.interpolate(other.rotation, w).R,
self.t + w * (other.t - self.t))
# return self.from_Rt(self.R * expm3(w * logm(self.R.T * other.R)), self.t + w * (other.t - self.t))
# return self.from_matrix(self.matrix * expm(w * logm((self.inverse() * other).matrix)))
def to_matrix(self):
""" Returns a 4x4 homogenous matrix of the form [R t; 0 1] """
result = self.quat.to_matrix()
result[:3, 3] = self.tvec
return result
def to_vector(self):
""" Returns a 7-vector representation of
rigid transform [tx, ty, tz, qx, qy, qz, qw]
"""
return np.hstack([self.tvec, self.xyzw])
def to_Rt(self):
""" Returns rotation R, and translational vector t """
T = self.to_matrix()
return T[:3, :3].copy(), T[:3, 3].copy()
def to_rpyxyz(self, axes='rxyz'):
r, p, y = self.quat.to_rpy(axes=axes)
return np.array((r, p, y, self.tvec[0], self.tvec[1], self.tvec[2]))
@classmethod
def from_rpyxyz(cls, roll, pitch, yaw, x, y, z, axes='rxyz'):
q = Quaternion.from_rpy(roll, pitch, yaw, axes=axes)
return cls(q, (x, y, z))
@classmethod
def from_Rt(cls, R, t):
T = np.eye(4)
T[:3, :3] = R.copy()
return cls(Quaternion.from_matrix(T), t)
@classmethod
def from_matrix(cls, T):
return cls(Quaternion.from_matrix(T), T[:3, 3])
@classmethod
def from_vector(cls, v):
return cls(xyzw=v[3:], tvec=v[:3])
@classmethod
def from_triad(cls, pos, v1, v2):
return RigidTransform.from_matrix(tf_compose(tf_construct(v1, v2),
pos))
@classmethod
def from_angle_axis(cls, angle, axis, tvec):
return cls(Quaternion.from_angle_axis(angle, axis), tvec)
@property
def wxyz(self):
return self.quat.wxyz
@property
def xyzw(self):
return self.quat.xyzw
@property
def R(self):
return self.quat.R
@property
def t(self):
return self.tvec
@property
def orientation(self):
return self.quat
@property
def rotation(self):
return self.quat
@property
def rpyxyz(self):
return self.to_rpyxyz()
@property
def translation(self):
return self.tvec
@classmethod
def identity(cls):
return cls()
@classmethod
def random(cls, t=1):
q_wxyz = [rnd.random(), rnd.random(), rnd.random(), rnd.random()]
qmag = np.sqrt(sum([x * x for x in q_wxyz]))
q_wxyz = [x / qmag for x in q_wxyz]
translation = [
rnd.uniform(-t, t),
rnd.uniform(-t, t),
rnd.uniform(-t, t)
]
return cls(Quaternion.from_wxyz(q_wxyz), translation)
@property
def matrix(self):
return self.to_matrix()
@property
def vector(self):
return self.to_vector()
def scaled(self, scale):
" Returns Sim3 representation of rigid-body transformation with scale "
return Sim3(xyzw=self.xyzw, tvec=self.tvec, scale=scale)
class RigidTransform(object):
"""
SE(3) rigid transform class that allows compounding of 6-DOF poses
and provides common transformations that are commonly seen in geometric problems.
quat: Quaternion/Rotation (xyzw)
tvec: Translation (xyz)
"""
def __init__(self, xyzw=[0.,0.,0.,1.], tvec=[0.,0.,0.]):
""" Initialize a RigidTransform with Quaternion and 3D Position """
self.quat = Quaternion(xyzw)
self.tvec = np.array(tvec)
def __repr__(self):
return 'rpy (rxyz): %s tvec: %s' % \
(np.array_str(self.quat.to_rpy(axes='rxyz'), precision=2, suppress_small=True),
np.array_str(self.tvec, precision=2, suppress_small=True))
# return 'quat: %s, tvec: %s' % (self.quat, self.tvec)
def __mul__(self, other):
"""
Left-multiply RigidTransform with another rigid transform
Two variants:
RigidTransform: Identical to oplus operation
ndarray: transform [N x 3] point set (X_2 = p_21 * X_1)
"""
if isinstance(other, RigidTransform):
return self.oplus(other)
else:
X = np.hstack([other, np.ones((len(other),1))]).T
return (np.dot(self.matrix, X).T)[:,:3]
def __rmul__(self, other):
raise NotImplementedError('Right multiply not implemented yet!')
# Basic operations
def inverse(self):
""" Returns a new RigidTransform that corresponds to the inverse of this one """
qinv = self.quat.inverse()
return RigidTransform(qinv, qinv.rotate(-self.tvec))
def oplus(self, other):
if isinstance(other, RigidTransform):
t = self.quat.rotate(other.tvec) + self.tvec
r = self.quat * other.quat
return RigidTransform(r, t)
elif isinstance(other, list):
return map(lambda o: self.oplus(o), other)
else:
raise TypeError("Type inconsistent", type(other), other.__class__)
def wrt(self, p_tr):
"""
self: p_ro, desired: p_to
o: other, r: reference, t: target
"""
return self * p_tr
def rotate_vec(self, v):
if v.ndim == 2:
return np.vstack(map(lambda v_: self.quat.rotate(v_), v))
else:
assert(v.ndim == 1 or (v.ndim == 2 and v.shape[0] == 1))
return self.quat.rotate(v)
def interpolate(self, other, w):
"""
LERP interpolation on rotation, and linear interpolation on position
Other approaches:
https://www.cvl.isy.liu.se/education/graduate/geometry-for-computer-vision-2014/geometry2014/lecture7.pdf
"""
assert(w >= 0 and w <= 1.0)
return self.from_Rt(self.rotation.interpolate(other.rotation, w).R, self.t + w * (other.t - self.t))
# return self.from_Rt(self.R * expm3(w * logm(self.R.T * other.R)), self.t + w * (other.t - self.t))
# return self.from_matrix(self.matrix * expm(w * logm((self.inverse() * other).matrix)))
# def interpolate(self, other_transform, this_weight):
# assert this_weight >= 0 and this_weight <= 1
# t = self.tvec * this_weight + other_transform.tvec * (1 - this_weight)
# r = self.quat.interpolate(other_transform.quat, this_weight)
# return RigidTransform(r, t)
# def ominus(self, other):
# if not isinstance(other, RigidTransform):
# raise TypeError("Type inconsistent")
# oinv = other.inverse()
# return oinv.oplus(self)
# (To) Conversions
def to_matrix(self):
""" Returns a 4x4 homogenous matrix of the form [R t; 0 1] """
result = self.quat.to_matrix()
result[:3, 3] = self.tvec
return result
def to_Rt(self):
""" Returns rotation R, and translational vector t """
T = self.to_matrix()
return T[:3,:3].copy(), T[:3,3].copy()
def to_rpyxyz(self, axes='rxyz'):
r, p, y = self.quat.to_rpy(axes=axes)
return np.array((r, p, y, self.tvec[0], self.tvec[1], self.tvec[2]))
# (From) Conversions
@classmethod
def from_rpyxyz(cls, roll, pitch, yaw, x, y, z, axes='rxyz'):
q = Quaternion.from_rpy(roll, pitch, yaw, axes=axes)
return cls(q, (x, y, z))
@classmethod
def from_Rt(cls, R, t):
T = np.eye(4)
T[:3,:3] = R.copy();
return cls(Quaternion.from_matrix(T), t)
@classmethod
def from_matrix(cls, T):
return cls(Quaternion.from_matrix(T), T[:3,3])
@classmethod
def from_triad(cls, pos, v1, v2):
# print v1, v2, type(v1)
return RigidTransform.from_matrix(tf_compose(tf_construct(v1, v2), pos))
@classmethod
def from_angle_axis(cls, angle, axis, tvec):
return cls(Quaternion.from_angle_axis(angle, axis), tvec)
# Properties
@property
def wxyz(self):
return self.quat.wxyz
@property
def xyzw(self):
return self.quat.xyzw
@property
def R(self):
return self.quat.R
@property
def t(self):
return self.tvec
@property
def orientation(self):
return self.quat
@property
def rotation(self):
return self.quat
@property
def translation(self):
return self.tvec
@classmethod
def identity(cls):
return cls()
@property
def matrix(self):
return self.to_matrix()
def make_random_transform(t=1):
q_wxyz = [ random.random(), random.random(), random.random(), random.random() ]
qmag = np.sqrt(sum([x*x for x in q_wxyz]))
q_wxyz = [ x / qmag for x in q_wxyz ]
translation = [ random.uniform(-t, t), random.uniform(-t, t), random.uniform(-t, t) ]
return RigidTransform(Quaternion.from_wxyz(q_wxyz), translation)
np.array_str(self.quat.to_rpy(axes='rxyz'), precision=2),
np.array_str(self.tvec, precision=2))
if __name__ == "__main__":
import random
def make_random_transform(t=1):
q_wxyz = [ random.random(), random.random(), random.random(), random.random() ]
qmag = np.sqrt(sum([x*x for x in q_wxyz]))
q_wxyz = [ x / qmag for x in q_wxyz ]
translation = [ random.uniform(-t, t), random.uniform(-t, t), random.uniform(-t, t) ]
return RigidTransform(Quaternion.from_wxyz(q_wxyz), translation)
q = Quaternion.identity()
t = [ 1, 2, 3 ]
m = RigidTransform(q, t)
print "m"
print m.to_matrix()
print "--------------------------"
q2 = Quaternion.from_rpy(np.pi / 4, 0, 0)
t2 = [ 0, 0, 0 ]
m2 = RigidTransform(q2, t2)
print "m2"
print m2.to_matrix()
print "--------------------------"
m3 = m * m2
print "m * m2"