ar_tfms, hydra_tfms = get_transforms(vals.marker, vals.hydra, vals.n_tfm, vals.n_avg)
# ar_tfms, hydra_tfms = get_hydra_transforms(vals.n_tfm, vals.n_avg)
# ar_tfms, hydra_tfms = get_ar_transforms(8,5, vals.n_tfm, vals.n_avg)
# print "AR Transforms: ", ar_tfms
# print "Hydra Transforms: ", hydra_tfms
# import cPickle as pickle
# pickle.dump({"AR":ar_tfms, "Hydra":hydra_tfms}, open( "transforms.p", "wb" ))
if vals.publish_tf:
T_ms = ss.solve4(ar_tfms, hydra_tfms)
T_chs = [ar_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(ar_tfms))]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_chs]
trans = np.asarray([trans for (trans, rot) in trans_rots])
avg_trans = np.sum(trans, axis=0) / trans.shape[0]
rots = [rot for (trans, rot) in trans_rots]
avg_rot = avg_quaternions(np.array(rots))
T_ch = conversions.trans_rot_to_hmat(avg_trans, avg_rot)
# T_ch = ar_tfms[0].dot(T_ms).dot(np.linalg.inv(hydra_tfms[0]))
print T_ch
# print T_ch.dot(hydra_tfms[0].dot(np.linalg.inv(T_ms))).dot(np.linalg.inv(ar_tfms[0]))
publish_tf(T_ch, 'camera1_link', 'hydra_base')
# publish_tf(, 'ar_marker_%d'%vals.marker, 'hydra_%s_pivot'%vals.hydra)
def get_transforms(marker, hydra, n_tfm , n_avg):
"""
Returns a tuple of two list of N_TFM transforms, each
the average of N_AVG transforms
corresponding to the AR marker with ID = MARKER and
the hydra paddle of the HYDRA ('right' or 'left') side.
"""
camera_frame = 'camera1_link'
hydra_frame = 'hydra_base'
marker_frame = 'ar_marker%d_camera1' % marker
assert hydra == 'right' or hydra == 'left'
paddle_frame = 'hydra_%s' % hydra
tf_sub = tf.TransformListener()
I_0 = np.eye(4)
I_0[3, 3] = 0
ar_tfms = []
hydra_tfms = []
for i in xrange(n_tfm + 1):
raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm), "red", True))
# # transforms which need to be averaged.
m_ts = np.empty((0, 3))
m_qs = np.empty((0, 4))
h_ts = np.empty((0, 3))
h_qs = np.empty((0, 4))
for j in xrange(n_avg):
print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True)
mtrans, mrot, htrans, hrot = None, None, None, None
sleeper = rospy.Rate(30)
while htrans == None:
# now = rospy.Time.now()
# tf_sub.waitForTransform(camera_frame, marker_frame, now)
# mtrans, mrot = tf_sub.lookupTransform(marker_frame, camera_frame, rospy.Time(0))
# (htrans,hrot) = tf_sub.lookupTransform(paddle_frame, hydra_frame, rospy.Time(0))
mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0))
htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
sleeper.sleep()
# try:
# tf_sub.waitForTransform(hydra_frame, paddle_frame, now, rospy.Duration(0.05))
# except (tf.LookupException):
# continue
m_ts = np.r_[m_ts, np.array(mtrans, ndmin=2)]
h_ts = np.r_[h_ts, np.array(htrans, ndmin=2)]
m_qs = np.r_[m_qs, np.array(mrot, ndmin=2)]
h_qs = np.r_[h_qs, np.array(hrot, ndmin=2)]
mtrans_avg = np.sum(m_ts, axis=0) / n_avg
mrot_avg = avg_quaternions(m_qs)
htrans_avg = np.sum(h_ts, axis=0) / n_avg
hrot_avg = avg_quaternions(h_qs)
# ar_tfm = tf_sub.fromTranslationRotation(mtrans_avg, mrot_avg)
# h_tfm = tf_sub.fromTranslationRotation(htrans_avg, hrot_avg)
ar_tfm = conversions.trans_rot_to_hmat(mtrans_avg, mrot_avg)
h_tfm = conversions.trans_rot_to_hmat(htrans_avg, hrot_avg)
print "\nar:"
print ar_tfm
print ar_tfm.dot(I_0).dot(ar_tfm.T)
print "h:"
print h_tfm
print h_tfm.dot(I_0).dot(h_tfm.T), "\n"
# ar_tfm = conversions.trans_rot_to_hmat(mtrans,mrot)
# h_tfm = conversions.trans_rot_to_hmat(htrans,hrot)
# print "\nar:"
# print ar_tfm
# print ar_tfm.dot(I_0).dot(ar_tfm.T)
# print "h:"
# print h_tfm
# print h_tfm.dot(I_0).dot(h_tfm.T), "\n"
ar_tfms.append(ar_tfm)
hydra_tfms.append(h_tfm)
return (ar_tfms, hydra_tfms)
k1_t, k1_q = conversions.hmat_to_trans_rot(tfm1)
k1_ts = np.r_[k1_ts, np.array(k1_t, ndmin=2)]
k1_qs = np.r_[k1_qs, np.array(k1_q, ndmin=2)]
rgb2 = cs2.readFrame()
depth2 = ds2.readFrame()
tfm2 = get_ar_transform_id(depth2.data, rgb2.data, 2)
#print tfm
#trans, rot = conversions.hmat_to_trans_rot(tfm)
print tfm2
if tfm2 == None:
print "got none"
continue
k2_t, k2_q = conversions.hmat_to_trans_rot(tfm2)
k2_ts = np.r_[k2_ts, np.array(k2_t, ndmin=2)]
k2_qs = np.r_[k2_qs, np.array(k2_q, ndmin=2)]
print i
i = i+1
#except:
# print "Warning: Problem with ar"
# pass
#cv2.waitKey(30)
k1_trans_avg = np.sum(k1_ts, axis=0) / args.n_tfm
k1_rot_avg = avg_quaternions(k1_qs)
k1_tfm = conversions.trans_rot_to_hmat(k1_trans_avg, k1_rot_avg)
print k1_tfm
#except KeyboardInterrupt:
# print "got Control-C"
