def finish_calibration(self, calib_type='camera'):
"""
Finds the final transform between parent_frame and hydra_base.
"""
if not self.calib_transforms or not self.hydra_transforms: return False
if len(self.calib_transforms) != len(self.hydra_transforms): return False
Tas = solve4(self.calib_transforms, self.hydra_transforms)
avg_hydra_base_transforms = [calib_tfm.dot(Tas).dot(np.linalg.inv(hydra_tfm)) for calib_tfm, hydra_tfm in zip(self.calib_transforms, self.hydra_transforms)]
tfm = {}
tfm['tfm'] = utils.avg_transform(avg_hydra_base_transforms)
if calib_type is 'camera':
pf = self.parent_frame[calib_type]
cname = pf.split('_')[0]
tfm['parent'] = cname+'_link'
tfm['tfm'] = tfm_link_rof.dot(tfm['tfm'])
else:
tfm['parent'] = self.parent_frame[calib_type]
tfm['child'] = 'hydra_base'
self.transforms[calib_type] = tfm
return True
def finish_calibration(self):
"""
Finds the final transform between parent_frame and hydra_base.
"""
if not self.ar_transforms or self.hydra_transforms: return False
if len(self.ar_transforms) != len(self.hydra_transforms): return False
Tas = solve4(self.ar_transforms, self.hydra_transforms)
avg_hydra_base_transforms = [ar_tfm.dot(Tas).dot(hydra_tfm) for ar_tfm, hydra_tfm in zip(self.ar_transforms, self.hydra_transforms)]
self.hydra_base_transform = utils.avg_transform(avg_hydra_base_transforms)
self.cameras.stop_streaming()
return True
def get_transform_kb (grabber, marker, n_avg=10, n_tfm=3, print_shit=True):
"""
Prompts the user to hit the keyboard whenever taking an observation.
Switch on phasespace before this.
"""
tfms_ar = []
tfms_ph = []
i = 0
while i < n_tfm:
raw_input("Hit return when ready to take transform %i."%i)
rgb, depth = grabber.getRGBD()
ar_tfm = get_ar_transform_id(depth, rgb, marker)
ph_tfm = get_phasespace_transform()
if ar_tfm is None:
print "Could not find AR marker %i. Try again." %marker
continue
if ph_tfm is None:
print "Could not correct phasespace markers. Try again."
continue
if print_shit:
print "\n ar: "
print ar_tfm
print ar_tfm.dot(I_0).dot(ar_tfm.T)
print "ph: "
print ph_tfm
print ph_tfm.dot(I_0).dot(ph_tfm.T)
tfms_ar.append(ar_tfm)
tfms_ph.append(ph_tfm)
i += 1
print "Found %i transforms. Calibrating..."%n_tfm
Tas = ss.solve4(tfms_ar, tfms_ph)
print "Done."
Tcp = tfms_ar[0].dot(Tas.dot(nlg.inv(tfms_ph[0])))
return Tcp
rospy.init_node('calib_hydra_pr2')
parser = argparse.ArgumentParser(description="Hydra Kinect Calibration")
parser.add_argument('--arm', help="in {'right', 'left'} : the pr2 arm to track.", required=True)
parser.add_argument('--hydra', help="in {'right', 'left'} : the hydra handle to track.", required=True)
parser.add_argument('--n_tfm', help="number of transforms to use for calibration.", type=int, default=5)
parser.add_argument('--n_avg', help="number of estimates of transform to use for averaging.", type=int, default=5)
parser.add_argument('--publish_tf', help="whether to publish the transform between hydra_base and camera_link", default=True)
vals = parser.parse_args()
arm_tfms, hydra_tfms, kinect_tfms, head_tfms = get_transforms(vals.arm, vals.hydra, vals.n_tfm, vals.n_avg)
if vals.publish_tf:
# Solving for transform between base_footprint and hydra_base
T_ms = ss.solve4(arm_tfms, hydra_tfms)
T_chs = [arm_tfms[i].dot(T_ms).dot(np.linalg.inv(hydra_tfms[i])) for i in xrange(len(arm_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)
# Average transform from gripper to hydra (T_gh)
T_grip_hydra = [np.linalg.inv(arm_tfms[i]).dot(T_ch).dot(hydra_tfms[i]) for i in xrange(vals.n_tfm)]
trans_rots = [conversions.hmat_to_trans_rot(tfm) for tfm in T_grip_hydra]
trans = np.asarray([trans for (trans, rot) in trans_rots])
def get_transform_freq (grabber, marker, freq=0.5, n_avg=10, n_tfm=3, print_shit=False):
"""
Stores data at frequency provided.
Switch on phasespace before this.
"""
tfms_ar = []
tfms_ph = []
I_0 = np.eye(4)
I_0[3,3] = 0
i = 0
wait_time = 5
print "Waiting for %f seconds before collecting data."%wait_time
time.sleep(wait_time)
avg_freq = 30
while i < n_tfm:
print "Transform %i"%(i+1)
j = 0
ar_tfms = []
ph_tfms = []
while j < n_avg:
rgb, depth = grabber.getRGBD()
ar_tfm = get_ar_transform_id(depth, rgb, marker)
ph_tfm = ph.marker_transform(0,1,2, ph.get_marker_positions())
if ar_tfm is None:
print colorize("Could not find AR marker %i."%marker,"red",True)
continue
if ph_tfm is None:
print colorize("Could not correct phasespace markers.", "red", True)
continue
print colorize("Got transform %i for averaging."%(j+1), "blue", True)
ar_tfms.append(ar_tfm)
ph_tfms.append(ph_tfm)
j += 1
time.sleep(1/avg_freq)
ar_avg_tfm = utils.avg_transform(ar_tfms)
ph_avg_tfm = utils.avg_transform(ph_tfms)
if print_shit:
print "\n ar: "
print ar_avg_tfm
print ar_avg_tfm.dot(I_0).dot(ar_avg_tfm.T)
print "ph: "
print ph_avg_tfm
print ph_avg_tfm.dot(I_0).dot(ph_avg_tfm.T)
tfms_ar.append(ar_avg_tfm)
tfms_ph.append(ph_avg_tfm)
i += 1
time.sleep(1/freq)
print "Found %i transforms. Calibrating..."%n_tfm
Tas = ss.solve4(tfms_ar, tfms_ph)
print "Done."
T_cps = [tfms_ar[i].dot(Tas).dot(np.linalg.inv(tfms_ph[i])) for i in xrange(len(tfms_ar))]
return utils.avg_transform(T_cps)
def get_transform_ros(marker, n_tfm, n_avg, freq=None):
"""
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 phasespace markers.
"""
camera_frame = 'camera_depth_optical_frame'
marker_frame = 'ar_marker_%d'%marker
ps_frame = 'ps_marker_transform'
tf_sub = tf.TransformListener()
I_0 = np.eye(4)
I_0[3,3] = 0
ar_tfms = []
ph_tfms = []
wait_time = 5
print "Waiting for %f seconds before collecting data."%wait_time
time.sleep(wait_time)
sleeper = rospy.Rate(30)
for i in xrange(n_tfm+1):
if freq is None:
raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm), "red", True))
else:
print colorize("Transform %d of %d."%(i, n_tfm), "red", True)
## transforms which need to be averaged.
ar_tfm_avgs = []
ph_tfm_avgs = []
for j in xrange(n_avg):
print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)
mtrans, mrot, ptrans, prot = None, None, None, None
while ptrans == None or mtrans == None:
mtrans, mrot = tf_sub.lookupTransform(camera_frame, marker_frame, rospy.Time(0))
ptrans, prot = tf_sub.lookupTransform(ph.PHASESPACE_FRAME, ps_frame, rospy.Time(0))
sleeper.sleep()
ar_tfm_avgs.append(conversions.trans_rot_to_hmat(mtrans,mrot))
ph_tfm_avgs.append(conversions.trans_rot_to_hmat(ptrans,prot))
ar_tfm = utils.avg_transform(ar_tfm_avgs)
ph_tfm = utils.avg_transform(ph_tfm_avgs)
# print "\nar:"
# print ar_tfm
# print ar_tfm.dot(I_0).dot(ar_tfm.T)
# print "h:"
# print ph_tfm
# print ph_tfm.dot(I_0).dot(ph_tfm.T), "\n"
ar_tfms.append(ar_tfm)
ph_tfms.append(ph_tfm)
if freq is not None:
time.sleep(1/freq)
print "Found %i transforms. Calibrating..."%n_tfm
Tas = ss.solve4(ar_tfms, ph_tfms)
print "Done."
T_cps = [ar_tfms[i].dot(Tas).dot(np.linalg.inv(ph_tfms[i])) for i in xrange(len(ar_tfms))]
return utils.avg_transform(T_cps)
