def publish(self):
if not self._initialized:
return
now = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = now
g_msg.header.frame_id = 'base_link'
gv_msg = PoseArray()
gv_msg.header.stamp = now
gv_msg.header.frame_id = 'base_link'
# alignment ...
q = tx.quaternion_from_euler(np.pi / 2, 0, -np.pi / 2)
for i in range(self._num_markers):
txn = self._xyz[i]
rxn = tx.quaternion_from_euler(*(self._rpy[i]))
rxn = tx.quaternion_multiply(rxn, q)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [self._tag_size]
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
g_msg.detections.append(msg)
gv_msg.poses.append(pwcs.pose.pose)
self._tfb.sendTransform(txn,
rxn,
now,
child='gt_tag_{}'.format(i),
parent='base_link')
self._gpub.publish(g_msg)
self._gvpub.publish(gv_msg)
def data_cb(self, joint_msg, detection_msgs):
try:
jorder = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
j_idx = [joint_msg.name.index(j) for j in jorder]
j = [joint_msg.position[i] for i in j_idx]
j = np.concatenate((j, [0])) # assume final joint is fixed
except Exception as e:
rospy.logerr_throttle(
1.0, 'Joint Positions Failed : {} \n {}'.format(e, joint_msg))
Xs = [np.eye(4) for _ in range(self._num_markers)]
vis = [False for _ in range(self._num_markers)]
if len(detection_msgs.detections) <= 0:
return
try:
for pm in detection_msgs.detections:
# pm.pose = pwcs
# pm.pose.pose = pwc
# pm.pose.pose.pose = p
# TODO : technically requires tf.transformPose(...) for robustness.
#pose = tf.transformPose(
ps = PoseStamped(header=pm.pose.header, pose=pm.pose.pose.pose)
# this transform should theoretically be painless + static
# Optionally, store the transform beforehand and apply it
ps = self._tfl.transformPose('stereo_optical_link', ps)
q = ps.pose.orientation
p = ps.pose.position
m_id = pm.id[0]
X = tx.compose_matrix(angles=tx.euler_from_quaternion(
[q.x, q.y, q.z, q.w]),
translate=[p.x, p.y, p.z])
i = self._m2i[m_id] # transform tag id to index
if i >= self._num_markers:
continue
self._i2m[i] = m_id
Xs[i] = X
vis[i] = True
self._seen[i] = True
except Exception as e:
rospy.logerr_throttle(1.0, 'TF Failed : {}'.format(e))
Xs = np.float32(Xs)
self._data.append((j, Xs, vis))
self._step += 1
rospy.loginfo_throttle(
1.0, 'Current Step : {}; vis : {}'.format(self._step, vis))
T = self._calib.eval_1(j, Xs, vis) # == (1, M, 4, 4)
now = rospy.Time.now()
m_msg = AprilTagDetectionArray()
m_msg.header.frame_id = 'base_link'
m_msg.header.stamp = now
pv_msg = PoseArray()
pv_msg.header.stamp = now
pv_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
if vis[i]:
txn = tx.translation_from_matrix(T[i])
rxn = tx.quaternion_from_matrix(T[i])
msg = AprilTagDetection()
msg.id = [self._i2m[i]]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
m_msg.detections.append(msg)
pv_msg.poses.append(pwcs.pose.pose)
self._pub.publish(m_msg)
self._vpub.publish(pv_msg)
def data_cb(self, joint_msg, detection_msgs):
# set info-mat param
# reorder joint information
try:
jorder = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
j_idx = [joint_msg.name.index(j) for j in jorder]
j = [joint_msg.position[i] for i in j_idx]
#j = np.random.normal(loc=j, scale=0.005)
j = np.concatenate((j, [0])) # assume final joint is fixed
except Exception as e:
rospy.logerr_throttle(
1.0, 'Joint Positions Failed : {} \n {}'.format(e, joint_msg))
if len(detection_msgs.detections) <= 0:
# nothing detected, stop
return
# form pose ...
p, q = fk(self._dh0, j)
# testing ; add motion noise
# p = np.random.normal(p, scale=0.01)
# dq = qmath.rq(s=0.01)
# q = qmath.qmul(dq, q)
if not self._initialized:
# save previous pose and don't proceed
self._p = p
self._q = q
#self._slam.initialize(np.concatenate([p,q], axis=0))
self._initialized = True
return
# form observations ...
# nodes organized as:
# [base_link, landmarks{0..m-1}, poses{0..p-1}]
zs = []
try:
for pm in detection_msgs.detections:
m_id = pm.id[0]
ps = PoseStamped(header=pm.pose.header, pose=pm.pose.pose.pose)
# TODO : compute+apply static transform?
ps = self._tfl.transformPose('stereo_optical_link', ps)
zp, zq = pmsg2pq(ps.pose)
# testing; add observation noise
# zp = np.random.normal(zp, scale=0.05)
# dzq = qmath.rq(s=0.05)
# zq = qmath.qmul(dzq, zq)
dz = np.concatenate([zp, zq], axis=0)
zi = self._m2i[m_id]
if zi >= self._num_markers:
continue
self._i2m[zi] = m_id
zs.append([zi, dz, self._omega.copy()])
except Exception as e:
rospy.logerr_throttle(1.0, 'TF Failed : {}'.format(e))
return
### semi-offline batch optimization
ox = 1.0 * self._omega
self._graph.append([p, q, zs])
if len(self._graph) >= self._batch_size:
n_poses = 1 + len(self._graph) # add 1 for 0-node constraint
nodes = []
edges = []
p_nodes = []
z_nodes = [[] for _ in range(self._num_markers)]
# add 0-node
nodes.append(np.asarray([0, 0, 0, 0, 0, 0, 1]))
for gi, g in enumerate(self._graph):
xi = 1 + self._num_markers + gi # current pose index
# pose info
gp, gq, gz = g
gx = np.concatenate([gp, gq], axis=0)
p_nodes.append(gx)
edges.append([0, xi, gx, ox])
# TODO : handle scenarios when a landmark didn't appear in 200 observations
for zi, dz, zo in gz:
# edge from motion index to re-computed landmark index
edges.append([xi, 1 + zi, dz, zo])
zp_a, zq_a = qmath.x2pq(qmath.xadd_rel(gx, dz, T=False))
z_nodes[zi].append([zp_a, zq_a])
# add landmark nodes
if self._zinit:
# use the previous initialization for landmarks
for zp, zq in self._z_nodes:
zx = np.concatenate([zp, zq], axis=0)
nodes.append(zx)
else:
#initialize landmarks with average over 200 samples
for zi, zn in enumerate(z_nodes):
zp, zq = zip(*zn)
zp = np.mean(zp, axis=0)
zq = qmath.qmean(zq)
zx = np.concatenate([zp, zq], axis=0)
nodes.append(zx)
lms = nodes[1:1 + self._num_markers]
lms = [qmath.x2pq(x) for x in lms]
lms_re = [lms[self._m2i[i]] for i in range(self._num_markers)]
self._pub_ze.publish(msgn(lms_re, rospy.Time.now()))
self._zinit = True
# add motion nodes
for zx in p_nodes:
nodes.append(zx)
#print "PRE:"
#print nodes[-self._num_markers:]
nodes = self._slam.optimize(nodes,
edges,
max_iter=100,
tol=1e-12,
min_iter=100)
#print "POST:"
#print nodes[-self._num_markers:]
lms = nodes[1:1 + self._num_markers] # landmarks
lms = [qmath.x2pq(x) for x in lms]
self._z_nodes = lms
lms_re = [lms[self._m2i[i]] for i in range(self._num_markers)]
self._pub_ze.publish(msgn(lms_re, rospy.Time.now()))
self._graph.clear()
stamp = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = stamp
g_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
msg = AprilTagDetection()
m_id = self._i2m[i]
msg.id = [m_id]
msg.size = [0.0] # not really a thing
msg.pose.pose.pose = pose(lms[i][0], lms[i][1])
msg.pose.header.frame_id = 'base_link'
msg.pose.header.stamp = stamp
g_msg.detections.append(msg)
self._gt_pub.publish(g_msg)
return
### END: semi-offline batch optimization
### online slam with "reasonable" initialization?
#if not self._zinit:
# # add entry ...
# gx = np.concatenate([p,q], axis=0)
# for z in zs:
# _, zi, dz, _ = z
# zi = zi - 2
# zp_a, zq_a = qmath.x2pq(qmath.xadd_rel(gx, dz, T=False))
# self._z_nodes[zi].append( (zp_a, zq_a) )
# # check entries ...
# ls = [len(e) for e in self._z_nodes]
# if np.all(np.greater_equal(ls, 100)):
# znodes = []
# for zi, zn in enumerate(self._z_nodes):
# zp, zq = zip(*zn)
# zp = np.mean(zp, axis=0)
# zq = qmath.qmean(zq)
# zx = np.concatenate([zp,zq], axis=0)
# znodes.append(zx)
# self._slam._nodes[2+zi] = zx
# self._slam.initialize(gx)
# self._p = p
# self._q = q
# self._zinit = True
# return
### END : online slam with reasonable initialization
#dp, dq = qmath.xrel(self._p, self._q, p, q)
#dx = np.concatenate([dp,dq], axis=0)
dx = np.concatenate([p, q], axis=0)
self._slam._nodes[0] = np.asarray([0, 0, 0, 0, 0, 0, 1],
dtype=np.float32)
# save p-q
#self._p = p
#self._q = q
# for zp, zq in self._z_nodes:
# zx = np.concatenate([zp,zq], axis=0)
# nodes.append(zx)
# form dz ...
mu = self._slam.step(x=dx, zs=zs)
mu = np.reshape(mu, [-1, 7])
ep, eq = qmath.x2pq(mu[1])
gtp, gtq = fk(self._dh, j)
#perr = np.subtract(ep, gtp)
#perr = np.linalg.norm(perr)
#qerr = qmath.T(qmath.qmul(qmath.qinv(gtq), eq))
#qerr = ((qerr + np.pi) % (2*np.pi)) - np.pi
#qerr = np.linalg.norm(qerr)
#epe, eqe = perr, qerr
#perr = np.subtract(p, gtp)
#perr = np.linalg.norm(perr)
#qerr = qmath.T(qmath.qmul(qmath.qinv(gtq), q))
#qerr = ((qerr + np.pi) % (2*np.pi)) - np.pi
#qerr = np.linalg.norm(qerr)
#pe, qe = perr, qerr
#print pe-epe, qe-eqe
ez = [qmath.x2pq(e) for e in mu[2:]]
stamp = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = stamp
g_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
msg = AprilTagDetection()
m_id = self._i2m[i]
msg.id = [m_id]
msg.size = [0.0] # not really a thing
msg.pose.pose.pose = pose(ez[i][0], ez[i][1])
msg.pose.header.frame_id = 'base_link'
msg.pose.header.stamp = stamp
g_msg.detections.append(msg)
self._gt_pub.publish(g_msg)
#ez = [unparametrize(e) for e in mu[1:]]
self._p, self._q = ep.copy(), eq.copy()
#self._p, self._q = p, q
t = joint_msg.header.stamp
self._pub_ee.publish(msg1(ep, eq, t))
self._pub_ze.publish(msgn(ez, t))
def publish(self):
now = rospy.Time.now()
if self._smooth:
# smooth change over time ...
if self._j_0 is None:
self._j_0 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
self._j_1 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
jpos = np.copy(self._j_0)
else:
self._m_i += 1
jpos = alerp(self._j_0, self._j_1, float(self._m_i) / self._m_n)
if self._m_i >= self._m_n:
self._j_0 = np.copy(self._j_1)
self._j_1 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
self._m_i = 0
else:
# completely random
jpos = np.random.uniform(low=-np.pi, high=np.pi, size=5)
if self._zero:
jpos *= 0
txn, rxn = fk(self._dh, np.concatenate( [jpos, [0]] ))
## base_link --> object
M07 = [tx.compose_matrix(translate = xyz, angles=rpy) for (xyz, rpy) in zip(self._xyz, self._rpy)]
## base_link --> stereo_optical_link
M06 = tx.compose_matrix(
angles = tx.euler_from_quaternion(rxn),
translate = txn) # T_0_6
M06R = tx.quaternion_matrix(rxn)
Rt = M06R[:3,:3].T
T = -np.matmul(Rt, np.reshape(txn, [3,1]))
M06i = np.zeros((4,4), dtype=np.float32)
M06i[:3,:3] = Rt
M06i[:3, 3] = T[:,0]
M06i[ 3, 3] = 1.0
m_msg = AprilTagDetectionArray()
m_msg.header.stamp = now
m_msg.header.frame_id = 'stereo_optical_link'
pv_msg = PoseArray()
pv_msg.header.stamp = now
pv_msg.header.frame_id = 'stereo_optical_link'
M67 = [np.matmul(M06i, M) for M in M07]
for i in range(self._num_markers):
if np.random.random() > self._p:
continue
M = M67[i]
txn = tx.translation_from_matrix(M)
rxn = tx.quaternion_from_matrix(M)
if self._noise > 0.0:
txn = np.random.normal(loc=txn, scale=self._noise)
h = np.random.uniform(-self._noise, self._noise)
ax = np.random.normal(size=3)
ax /= np.linalg.norm(ax)
d_rxn = tx.quaternion_about_axis(h, ax)
rxn = tx.quaternion_multiply(d_rxn, rxn)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'stereo_optical_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
m_msg.detections.append(msg)
pv_msg.poses.append(pwcs.pose.pose)
self._ppub.publish(m_msg)
self._pvpub.publish(pv_msg)
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = now
g_msg.header.frame_id = 'base_link'
gv_msg = PoseArray()
gv_msg.header.stamp = now
gv_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
M = M07[i]
txn = tx.translation_from_matrix(M)
rxn = tx.quaternion_from_matrix(M)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
g_msg.detections.append(msg)
gv_msg.poses.append(pwcs.pose.pose)
if self._ground_truth:
self._gpub.publish(g_msg)
# visualizations are always published
self._gvpub.publish(gv_msg)
#gmsg = PoseStamped()
#gmsg.header.frame_id = 'base_link'
#txn = tx.translation_from_matrix(M07)
#rxn = tx.quaternion_from_matrix(M07)
#gmsg.header.stamp = now
#fill_pose_msg(gmsg.pose, txn, rxn)
#self._gpub.publish(gmsg)
jmsg = JointState()
jmsg.header.stamp = now
jmsg.header.frame_id = 'map' #??
jmsg.name = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
jmsg.position = jpos
jmsg.velocity = np.zeros_like(jpos)
jmsg.effort = np.zeros_like(jpos)
self._jpub.publish(jmsg)