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"