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)