Ejemplo n.º 1
0
    def get_tfm_from_obs(self, hydra_marker, n_tfm=15, n_avg=30):
        """
        Stores a bunch of readings from sensors.
        """
        all_tfms = []
        
        if rospy.get_name == '/unnamed':
            rospy.init_node('gripper_calib')
        sleeper = rospy.Rate(30)
    
        i = 0
        n_attempts_max = n_avg*2
        while i < n_tfm:
            raw_input(colorize("Getting transform %i out of %i. Hit return when ready."%(i, n_tfm-1), 'yellow', True))

            j = 0
            n_attempts_effective = 0;
            hyd_tfms = []
            ar_tfms = []
            while j < n_attempts_max:
                j += 1
                ar_tfms_j = self.cameras.get_ar_markers(markers=[self.ar_marker]);
                hyd_tfms_j = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=[hydra_marker]);
                
                if not ar_tfms_j or not hyd_tfms_j:
                    if not ar_tfms_j:
                        yellowprint("Could not find required ar markers from " + str(self.ar_marker))
                    else:
                        yellowprint("Could not find required hydra transforms from " + str(hydra_marker))
                    continue
                                
                ar_tfms.append(ar_tfms_j[self.ar_marker])
                hyd_tfms.append(hyd_tfms_j[hydra_marker])
                
                blueprint('\tGetting averaging transform : %d of %d ...'%(n_attempts_effective, n_avg-1))
                n_attempts_effective += 1 
                
                if n_attempts_effective == n_avg:
                    break
                
                sleeper.sleep()
            
            if n_attempts_effective < n_avg:
                yellowprint("Not enough transforms were collected; try again")
                continue

            ar_tfm = avg_transform(ar_tfms)
            hyd_tfm = avg_transform(hyd_tfms)

            all_tfms.append(nlg.inv(ar_tfm).dot(hyd_tfm))
            i += 1
            
        return avg_transform(all_tfms)
def compute_relative_transforms (masterGraph, init=None):
    """
    Takes in a transform graph @G such that it has enough data to begin calibration (is_ready(G) returns true).
    Optimizes and computes final relative transforms between all nodes (markers).
    Returns a graph final_G with the all edges (clique) and final transforms stored in edges.
    Make sure the graph is ready before this by calling is_ready(graph,min_obs).
    """

    new_mG = nx.DiGraph()
    graph_map = {} 
    for group in masterGraph.nodes_iter():
        G = masterGraph.node[group]["graph"]
        for i,j in G.edges_iter():
            G[i][j]['avg_tfm'] = utils.avg_transform(G[i][j]['transform_list'])
        # Optimize rigid body transforms.
        graph_map[G] = optimize_transforms(G)
        new_mG.add_node(group)
        new_mG.node[group]["graph"] = graph_map[G]
        if masterGraph.node[group].get("master_marker"):
            new_mG.node[group]["master_marker"] = masterGraph.node[group].get("master_marker")
        new_mG.node[group]["angle_scale"] = masterGraph.node[group]["angle_scale"]
        new_mG.node[group]["markers"] = masterGraph.node[group]["markers"]
        
        new_mG.node[group]["hydras"] = masterGraph.node[group]["hydras"]
        new_mG.node[group]["ar_markers"] = masterGraph.node[group]["ar_markers"]

        masterGraph.node[group]["primary"] = masterGraph.node[group]["graph"].nodes()[0]
        new_mG.node[group]["primary"] = masterGraph.node[group]["primary"]

    for g1,g2 in masterGraph.edges_iter():
        mg1  = graph_map[masterGraph.node[g1]["graph"]]
        mg2  = graph_map[masterGraph.node[g2]["graph"]]
        new_mG.add_edge(g1, g2)

        node1 = new_mG.node[g1].get("primary")
        node2 = new_mG.node[g2].get("primary")
        
        new_mG[g1][g2]['avg_tfm'] = {}
        for angle, tfm_data in masterGraph[g1][g2]['transform_list'].items():
            transforms = []
            # Converting all transforms to standard transform between two fixed nodes.
            for tfm in tfm_data:
                transforms.append(mg1.edge[node1][tfm['from']]['tfm'].dot(tfm['tfm'])\
                                  .dot(mg2.edge[tfm['to']][node2]['tfm']))
            new_mG[g1][g2]['avg_tfm'][angle] = utils.avg_transform(transforms)

    # Optimize over angles to get master graph with transforms.
    mG_opt = optimize_master_transforms(new_mG, init)
    
    return mG_opt            
    def get_all_transforms(self, marker_tfms, theta, parent_frame):
        """
        From marker transforms given in parent_frame, get all transforms
        of all markers on gripper.
        """
        
        ret_tfms = []
        
        cor_avg_tfms = []
        for m,tfm in marker_tfms.items():
            if m in self.allmarkers:
                cor_avg_tfms.append(tfm.dot(self.get_rel_transform(m,'cor', theta)))
        
        if len(cor_avg_tfms) == 0: return ret_tfms
        
        cor_tfm = utils.avg_transform(cor_avg_tfms)
        ret_tfms.append({'parent':parent_frame, 
                         'child':'%sgripper_%s'%(self.lr, 'cor'),
                         'tfm':cor_tfm})

        for m in self.allmarkers:
            if m != 'cor':
                tfm = self.get_rel_transform('cor', m, theta)
                ret_tfms.append({'parent':parent_frame, 
                                 'child':'%sgripper_%s'%(self.lr, m),
                                 'tfm':cor_tfm.dot(tfm)})

        if self.tt_calculated:
                tfm = self.get_rel_transform('cor', 'tool_tip', theta)
                ret_tfms.append({'parent':parent_frame, 
                                 'child':'%sgripper_%s'%(self.lr, 'tool_tip'),
                                 'tfm':cor_tfm.dot(tfm)})

        return ret_tfms
    def get_tool_tip_transform(self, marker_tfms, theta):
        """
        Get tool tip transfrom from dict of visible markers to transforms
        Also provide the angle of gripper (half of what is seen by pot)
        Don't need it if only master transform is visible.
        m has to be on one of the 'master,'l' or 'r' groups
        """
        if self.tt_calculated is False:
            redprint ("Tool tip transform not calibrated.")
            return

        masterg = self.transform_graph.node['master']['graph']

        avg_tfms = []
        
        for m in marker_tfms:
            if m in self.mmarkers:
                tt_tfm = masterg.edge[m]['tool_tip']['tfm']
            elif m in self.lmarkers:
                tt_tfm = self.transform_graph.edge['l']['master']['tfm_func'](m,'tool_tip', theta)
            elif m in self.rmarkers:
                tt_tfm = self.transform_graph.edge['r']['master']['tfm_func'](m,'tool_tip', theta)
            else:
                redprint('Marker not on gripper.')
                return
            avg_tfms.append(marker_tfms[m].dot(tt_tfm))
            
        return utils.avg_transform(avg_tfms)
Ejemplo n.º 5
0
    def get_markers_transform (self, markers, marker_tfms, theta):
        """
        Takes in marker_tfms found, angles and markers for
        which transforms are required.
        Returns a dict of marker to transforms for the markers
        requested, in the same frame as the marker tfms received.
        
        """
        
        rtn_tfms = {}
        cor_avg_tfms = []
        for m, tfm in marker_tfms.items():
            if m in self.allmarkers:
                cor_avg_tfms.append(tfm.dot(self.get_rel_transform(m, 'cor', theta)))
        
        cor_tfm = utils.avg_transform(cor_avg_tfms)

        for marker in markers:
            if marker in marker_tfms:
                rtn_tfms[marker] = marker_tfms[marker]
                continue
            
            tfm = self.get_rel_transform('cor', marker, theta)
            if tfm is not None:
                rtn_tfms[marker] = cor_tfm.dot(tfm)
        
        return rtn_tfms
def get_ar_markers_ros (cameras, parent_frame = None, cams = None, markers=None):
    """
    Returns all the ar_markers in the frame of camera 0.
    This would potentially take a while.
    Do this after all collecting data from everywhere else.
    cams -> list of cameras to use
    """
    if parent_frame is None:
        parent_frame = cameras.parent_frame

    ar_markers = {}
    
    data = cameras.get_RGBD(cams)
    for cam in data:
        tfm = cameras.get_transform_frame(cam, parent_frame)
        
        ar_cam = get_ar_marker_poses(data[cam]['rgb'], data[cam]['depth'])
        for marker in ar_cam:    
            if ar_markers.get(marker) is None:
                ar_markers[marker] = []
            ar_markers[marker].append(tfm.dot(ar_cam[marker]))
    
    for marker in ar_markers:
        ar_markers[marker] = utils.avg_transform(ar_markers[marker])
    
    if markers is None:
        return ar_markers
    else:
        return {marker:ar_markers[marker] for marker in ar_markers if marker in markers}
Ejemplo n.º 7
0
    def get_tooltip_transform(self, marker_tfms, theta):
        """
        Get tool tip transform from dict of relevant markers to transforms
        Also provide the angle of gripper.
        Don't need it if only master transform is visible.
        m has to be on one of the 'master,'l' or 'r' groups
        """
        if self.tooltip_calculated is False:
            redprint ("Tool tip transform not calibrated.")
            return

        masterg = self.transform_graph.node['master']['graph']

        avg_tfms = []
        
        for m in marker_tfms:
            if m in self.mmarkers:
                tt_tfm = masterg.edge[m]['tool_tip']['tfm']
            elif m in self.lmarkers:
                tt_tfm = self.transform_graph.edge['l']['master']['tfm_func'].get_tfm(m, 'tool_tip', theta)
            elif m in self.rmarkers:
                tt_tfm = self.transform_graph.edge['r']['master']['tfm_func'].get_tfm(m, 'tool_tip', theta)
            else:
                redprint('Marker %s not on gripper.' % m)
                continue
            avg_tfms.append(marker_tfms[m].dot(tt_tfm))
            
        if len(avg_tfms) == 0:
            redprint('No markers on gripper found.')
            return

        return utils.avg_transform(avg_tfms)
    def process_observation_ar(self, n_avg):
        """
        Get an observation and update transform list.
        n_avg is the number of observations used to compute a transform
        """
        
        # {ar markers for camera i}
        self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}
        
        sleeper = rospy.Rate(30)
        for i in xrange(n_avg):
            for cam_id in xrange(self.num_cameras):
                greenprint("Averaging %d out of %d for camera %i" % (i + 1, n_avg, cam_id + 1), False)
                tfms = self.cameras.get_ar_markers(camera=cam_id)
                for marker in tfms: 
                    if marker not in self.observed_ar_transforms[cam_id]:
                        self.observed_ar_transforms[cam_id][marker] = []
                    self.observed_ar_transforms[cam_id][marker].append(tfms[marker])
            sleeper.sleep()

        for cam_id in self.observed_ar_transforms:
            for marker in self.observed_ar_transforms[cam_id]:
                self.observed_ar_transforms[cam_id][marker] = utils.avg_transform(self.observed_ar_transforms[cam_id][marker])        

        observation_updated = False
        for i in xrange(1, self.num_cameras):
            result = self.extend_camera_pointsets_ar(0, i)
            if result is False:
                redprint("Did not get info for cameras 0 and %d" % i)
                continue
            observation_updated = True
        
        return observation_updated
    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 get_transforms(arm, 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 left/ right arm of the pr2 and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    
    return arm_tfms, hydra_tfms
    """

    pr2_frame = 'base_footprint'
    assert arm == 'right' or 'left'
    arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]

    hydra_frame = 'hydra_base'
    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
    
    gripper_tfms = []
    hydra_tfms = []

    for i in xrange(n_tfm):
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms" % (i, n_tfm - 1), "red", True))
        
        # # transforms which need to be averaged.
        gtfms = []
        htfms = []
        
        sleeper = rospy.Rate(30)        
        for j in xrange(n_avg):            
            print colorize('\tGetting averaging transform : %d of %d ...' % (j, n_avg - 1), "blue", True)

            gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
            gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot))
            htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
            htfms.append(conversions.trans_rot_to_hmat(htrans, hrot))
            sleeper.sleep()
            
        gripper_tfms.append(avg_transform(gtfms))
        hydra_tfms.append(avg_transform(htfms))

    return (gripper_tfms, hydra_tfms)
    def process_observation(self, n_avg=5):
        """
        Store an observation of ar_marker and hydras.
        """
        
        raw_input(colorize("Press return when ready to capture transforms.", "green", True))
        
        ar_avg_tfm = []
        hydra_avg_tfm = []
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)

            ar_transforms = gmt.get_ar_markers_from_cameras(self.cameras, cams = [self.calib_camera], markers = [self.ar_marker])
            hydra_transforms = gmt.get_hydra_transforms('hydra_base', [self.calib_hydra])
            ar_avg_tfm.append(ar_transforms[self.ar_marker])
            hydra_avg_tfm.append(hydra_transforms[self.ar_marker][self.calib_hydra])
            
        self.ar_transforms.append(utils.avg_transform(ar_avg_tfm))
        self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
Ejemplo n.º 12
0
    def get_ar_markers (self, markers=None, camera=None, parent_frame=False, get_time=False):
        """
        @markers is a list of markers to be found. Default of None means all markers.
        @camera specifies which camera to use. Default of None means all cameras are used 
            (assuming calibrated).
        @parent_frame specifies whether the transforms are in the parent frame or camera frame
            (assuming calibrated).
        
        
        UGLY FUNCTION -- BREAK IT INTO BETTER FUNCTIONS.
        """
        if camera is None:
            if not self.calibrated:
                redprint('Cameras not calibrated. Cannot get transforms from all cameras.')
                return {}
            
            marker_tfms = {}
            time_stamp = 0.0
            num_seen = 0
            for i in range(self.num_cameras):
                ctfm = self.get_camera_transform(0, i)
                tfms, t = self.camera_markers[i].get_marker_transforms(markers, get_time=True)
                if tfms: 
                    time_stamp += t
                    num_seen += 1
                
                
                for marker in tfms:
                    if marker not in marker_tfms:
                        marker_tfms[marker] = []
                    marker_tfms[marker].append(ctfm.dot(tfms[marker]))
                
            for marker in marker_tfms:
                marker_tfms[marker] = utils.avg_transform(marker_tfms[marker])
            if num_seen > 0:
                time_stamp = time_stamp / num_seen
        else:
            assert camera in range(self.num_cameras)
            marker_tfms, time_stamp = self.camera_markers[camera].get_marker_transforms(markers, get_time=True)
            if parent_frame is True:
                if not self.calibrated:
                    redprint('Cameras not calibrated. Cannot get transforms from all cameras.')
                    return {}
                ctfm = self.get_camera_transform(0, camera)
                for marker, tfm in marker_tfms.items():
                    marker_tfms[marker] = ctfm.dot(tfm) # all transforms in reference camera's frame

        if get_time:
            return marker_tfms, time_stamp
        else:
            return marker_tfms
 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 process_observation(self, n_avg=5):
        """
        Store an observation of ar_marker and hydras.
        """        
        raw_input(colorize("Press return when ready to capture transforms.", "green", True))
        
        calib_avg_tfm = []
        hydra_avg_tfm = []
        j = 0
        thresh = n_avg*2
        
        sleeper = rospy.Rate(30)
        while j < n_avg:
            print colorize('\tGetting averaging transform : %d of %d ...'%(j+1,n_avg), "blue", True)

            calib_tfm = self.calib_func()
            hydra_tfm = self.get_hydra_transform()
            if calib_tfm is None or hydra_tfm is None:
                if calib_tfm is None:
                    yellowprint('Could not find all required AR transforms.')
                else:
                    yellowprint('Could not find all required hydra transforms.')
                thresh -= 1
                if thresh == 0:
                    redprint('Tried too many times but could not get enough transforms.')
                    return False
                
                continue
                
            calib_avg_tfm.append(calib_tfm)
            hydra_avg_tfm.append(hydra_tfm)
            j += 1
            sleeper.sleep()

        self.calib_transforms.append(utils.avg_transform(calib_avg_tfm))
        self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
        return True
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        sleeper = rospy.Rate(30)
        avg_tfms = {}
        j = 0
        thresh = n_avg*2
        pot_avg = 0.0
        while j < n_avg:
            blueprint('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1))    

            tfms = {}
            # F**k the frames bullshit
            ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers)
            hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
            pot_angle = gmt.get_pot_angle()
            
            if not ar_tfm or (not hyd_tfm and self.hydras):
                yellowprint('Could not find all required transforms.')
                thresh -= 1
                if thresh == 0: return False
                continue
            
            pot_avg += pot_angle
            
            j += 1
            tfms.update(ar_tfm)
            tfms.update(hyd_tfm)

            # The angle relevant for each finger is only half the angle.

                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
                
            sleeper.sleep()
            
        pot_avg /= n_avg
        
        greenprint("Average angle found: %f"%pot_avg)

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
        return True
    def finish_calibration_pycb(self, avg=False):
        
        cb_transforms = {i:[] for i in range(self.num_cameras)}
        I = np.eye(4)
        
        for i in self.image_list:
            yellowprint("Getting transform data for camera %i." % (i + 1))
            ind = 1
            for img in self.image_list[i]:
                blueprint("... Observation %i." % ind)
                ind += 1

                tfm = cu.get_checkerboard_transform(img, chessboard_size=chessboard_size)
                if tfm is None: return False
                cb_transforms[i].append(tfm)
        
        rel_tfms = {i:[] for i in range(1, self.num_cameras)}
        for i in range(1, self.num_cameras):
            cam_transform = {}
            cam_transform['parent'] = 'camera1_link'
            cam_transform['child'] = 'camera%d_link' % (i + 1)
            for j in range(len(cb_transforms[i])):
                rtfm = cb_transforms[0][j].dot(nlg.inv(cb_transforms[i][j]))
                rel_tfms[i].append(rtfm)
            if avg:
                tfm = utils.avg_transform(rel_tfms[i])
                # convert from in to cm
                tfm[0:3, 3] *= 1.0  # 0.0254
                cam_transform['tfm'] = tfm_link_rof.dot(tfm).dot(np.linalg.inv(tfm_link_rof))

            else:
                scores = []
                for j, rtfm in enumerate(rel_tfms[i]):
                    scores.append(0)
                    rtfm = rel_tfms[i][j]
                    for k, rtfm2 in enumerate(rel_tfms[i]):
                        if j == k:
                            continue
                        scores[j] += nlg.norm(rtfm.dot(nlg.inv(rtfm2)) - I)
                print "Scores:", scores
                tfm = rel_tfms[i][np.argmin(scores)]
                # convert from in to m
                tfm[0:3, 3] *= 1.0  # 0.0254
                cam_transform['tfm'] = tfm_link_rof.dot(tfm).dot(np.linalg.inv(tfm_link_rof))
            self.camera_transforms[0, i] = cam_transform
        return True
def quick_calibrate(NUM_CAM, N_AVG):

    
    rospy.init_node('quick_calibrate')
    
    cameras = RosCameras(NUM_CAM)
    tfm_pub = tf.TransformBroadcaster()
    
    frames = {i:'/camera%i_link'%(i+1) for i in xrange(NUM_CAM)}
    
    calib_tfms = {(frames[0],frames[i]):None for i in xrange(1,NUM_CAM)}
    
    sleeper = rospy.Rate(30)
    
    try:
        while True:
            tfms_found = {i:{} for i in xrange(NUM_CAM)}
            for _ in xrange(N_AVG):
                for i in xrange(NUM_CAM):
                    mtfms = cameras.get_ar_markers(camera=i)
                    for m in mtfms:
                        if m not in tfms_found[i]:
                            tfms_found[i][m] = []
                        tfms_found[i][m].append(mtfms[m])
            
            for i in tfms_found:
                for m in tfms_found[i]:
                    tfms_found[i][m] = utils.avg_transform(tfms_found[i][m])        

            for i in xrange(1,NUM_CAM):
                rof_tfm = find_transform(tfms_found[0], tfms_found[i])
                if rof_tfm is not None:
                    calib_tfms[(frames[0], frames[i])] = tfm_link_rof.dot(rof_tfm).dot(nlg.inv(tfm_link_rof))

            for parent, child in calib_tfms:
                if calib_tfms[parent, child] is not None:
                    trans, rot = conversions.hmat_to_trans_rot(calib_tfms[parent, child])
                    tfm_pub.sendTransform(trans, rot,
                                          rospy.Time.now(),
                                          child, parent)

            sleeper.sleep()

    except KeyboardInterrupt:
        print "got ctrl-c"
    def finish_calibration(self):
        """
        Average out transforms and store final values.
        Return true/false based on whether transforms were found. 
        """
        if not self.transform_list: return False

        for c1,c2 in self.transform_list:
            cam_transform= {}
            cam_transform['tfm'] = utils.avg_transform(self.transform_list[key])
            cam_transform['parent'] = 'camera%d_depth_optical_frame'%(c1+1)
            cam_transform['child'] = 'camera%d_depth_optical_frame'%(c2+1)
            self.camera_transforms[c1,c2] = cam_transform
            
        self.cameras.stop_streaming()
        self.cameras.store_calibrated_transforms(self.camera_transforms)
    
        return True
def get_avg_hydra_tfm(lr, n_avg, sleeper):
    """
    Returns averaged hydra tfms.
    """
    if not isinstance(lr, list): lr = [lr]

    avg_tfms = {h:[] for h in lr}
    
    j = 0
    while j < n_avg:
        hydra_tfms = gmt.get_hydra_transforms('hydra_base', lr)
        if not hydra_tfms:
            continue
        for h in hydra_tfms:
            avg_tfms[h].append(hydra_tfms[h])
        j += 1
        sleeper.sleep()
    for h in lr: avg_tfms[h] = avg_transform(avg_tfms[h])
 
    return avg_tfms
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        avg_tfms = {}
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)            

            tfms = {}
            tfms.update(gmt.get_ar_markers_from_cameras(self.cameras, parent_frame=self.parent_frame, markers=self.ar_markers))
            tfms.update(gmt.get_hydra_transforms(parent_frame=parent_frame, hydras = self.hydras))

            pot_angle = gmt.get_pot_angle()
                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
        
        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
    def estimate_initial_transform(self):
        '''
        When using chessboard for calibration, there may be ambiguity due to the symmetric of chessboard.
        As a result, AR markers are used to remove ambiguity 
        '''
        raw_input("Place AR marker(s) visible to all cameras to get an 'initial' estimate. Then hit enter.")
        N_AVG = 10

        self.est_tfms = {cam_id:{} for cam_id in xrange(1, self.num_cameras)}
        tfms_found = {cam_id:{} for cam_id in xrange(self.num_cameras)}
        sleeper = rospy.Rate(30)
        for _ in xrange(N_AVG):
            for cam_id in xrange(self.num_cameras):
                mtfms = self.cameras.get_ar_markers(camera=cam_id)
                for m in mtfms:
                    if m not in tfms_found[cam_id]:
                        tfms_found[cam_id][m] = []
                    tfms_found[cam_id][m].append(mtfms[m])
            sleeper.sleep()
            
        for cam_id in tfms_found:
            for m in tfms_found[cam_id]:
                tfms_found[cam_id][m] = utils.avg_transform(tfms_found[cam_id][m])

        for cam_id in xrange(1, self.num_cameras):
            ar1, ar2 = common_ar_markers(tfms_found[0], tfms_found[cam_id])
            if not ar1 or not ar2:
                redprint("No common AR Markers found between camera 1 and %i" % (cam_id + 1))
                return False

            if len(ar1.keys()) == 1:
                self.est_tfms[0, cam_id] = ar1.values()[0].dot(nlg.inv(ar2.values()[0]))
            else:
                self.est_tfms[0, cam_id] = find_rigid_tfm(convert_hmats_to_points(ar1.values(), True),
                                                          convert_hmats_to_points(ar2.values(), True))
        
        return True
    def process_observation(self, n_avg=5):
        """
        Get an observation and update transform list.
        """
        
        yellowprint("Please hold still for a few seconds.")

        self.observation_info = {i:[] for i in xrange(self.num_cameras)}
        self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}

        # Get RGBD observations
        for i in xrange(n_avg):
            print colorize("Transform %d out of %d for averaging."%(i,n_avg),'yellow',False)
            data = self.cameras.get_RGBD()
            for j,cam_data in data.items():
                self.observation_info[j].append(cam_data)
        
        # Find AR transforms from RGBD observations and average out transforms.
        for i in self.observation_info:
            for obs in self.observation_info[i]:
                ar_pos = gmt.get_ar_marker_poses (obs['rgb'], obs['depth'])
                for marker in ar_pos:
                    if self.observed_ar_transforms[i].get(marker) is None:
                        self.observed_ar_transforms[i][marker] = []
                    self.observed_ar_transforms[i][marker].append(ar_pos[marker])
            for marker in self.observed_ar_transforms[i]:
                self.observed_ar_transforms[i][marker] = utils.avg_transform(self.observed_ar_transforms[i][marker]) 

        for i in xrange(1,self.num_cameras):
            transform = self.find_transform_between_cameras_from_obs(0, i)
            if transform is None:
                redprint("Did not find a transform between cameras 0 and %d"%i)
                continue
            if self.transform_list.get(0,i) is None:
               self.transform_list[0,i] = []
            self.transform_list[0,i].append(transform)
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)
Ejemplo n.º 24
0
    def get_all_transforms(self, parent_frame, diff_cam=False):
        """
        From marker transforms given in parent_frame, get all transforms
        of all markers on gripper.
        """
    	if self.cameras is None:
    	    redprint("Cameras not initialized. Could not get transforms.")
    	    return

        ar_tfms = self.cameras.get_ar_markers()

        if diff_cam:
            ar_tfms_cam = {}
            for i in range(self.cameras.num_cameras):
                ar_tfms_cam[i] = self.cameras.get_ar_markers(camera=i, parent_frame=True)

        hyd_tfms = gmt.get_hydra_transforms(self.parent_frame, None)
        theta = gmt.get_pot_angle(self.lr)

        marker_tfms = ar_tfms
        marker_tfms.update(hyd_tfms)
        
        ret_tfms = []
        
        cor_avg_tfms = []
        cor_hyd_avg = []
        cor_ar_avg = []
        ar_found = False
        hyd_found = False
        for m, tfm in marker_tfms.items():
            if m in self.ar_markers:
                c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) 
                cor_ar_avg.append(c_tfm)
                cor_avg_tfms.append(c_tfm)
                ar_found = True
            elif m in self.hydra_markers:
                c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) 
                cor_hyd_avg.append(c_tfm)
                cor_avg_tfms.append(c_tfm)
                hyd_found = True
        
        if diff_cam:
            cor_ar_cams = {}
            cam_found = {i:False for i in ar_tfms_cam}
            for i in ar_tfms_cam:
                for m, tfm in ar_tfms_cam[i].items():
                    if m in self.ar_markers:
                        c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta))
                        if i not in cor_ar_cams:
                            cor_ar_cams[i] = []
                            cam_found[i] = True 
                        cor_ar_cams[i].append(c_tfm)
                if cam_found[i]:
                    cor_ar_cams[i] = utils.avg_transform(cor_ar_cams[i])


        if len(cor_avg_tfms) == 0: return ret_tfms
        
        cor_tfm = utils.avg_transform(cor_avg_tfms)
        cor_h_tfm = utils.avg_transform(cor_hyd_avg)
        cor_a_tfm = utils.avg_transform(cor_ar_avg)
        
        ret_tfms.append({'parent':parent_frame,
                         'child':'%sgripper_%s' % (self.lr, 'cor'),
                         'tfm':cor_tfm})

        for m in self.allmarkers:
            if m != 'cor' and m != 'tool_tip':
                tfm = self.get_rel_transform('cor', m, theta)
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_%s' % (self.lr, m),
                                 'tfm':cor_tfm.dot(tfm)})

        if self.tooltip_calculated:
            tfm = self.get_rel_transform('cor', 'tool_tip', theta)
            ret_tfms.append({'parent':parent_frame,
                             'child':'%sgripper_tooltip' % self.lr,
                             'tfm':cor_tfm.dot(tfm)})
            if hyd_found:
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_tooltip_hydra' % self.lr,
                                 'tfm':cor_h_tfm.dot(tfm)})
            if ar_found:
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_tooltip_ar' % self.lr,
                                 'tfm':cor_a_tfm.dot(tfm)})
            
            if diff_cam:
                for i, cor_tfm in cor_ar_cams.items():
                    ret_tfms.append({'parent':parent_frame,
                                     'child':'%sgripper_tooltip_camera%i' % (self.lr, i + 1),
                                     'tfm':cor_tfm.dot(tfm)})
                    
        
        return ret_tfms
Ejemplo n.º 25
0
    def get_obs_new_marker(self, marker, n_tfm=10, n_avg=10):
        """
        Store a bunch of readings seen for new marker being added.
        Returns a list of dicts each of which has marker transforms found
        and potentiometer angles.
        """
        
        all_obs = []
        
        i = 0
        thresh = n_avg * 2
        sleeper = rospy.Rate(30)
        
        while i < n_tfm:
            raw_input(colorize("Getting transform %i out of %i. Hit return when ready." % (i, n_tfm), 'yellow', True))
            
            j = 0
            j_th = 0
            pot_angle = 0
            found = True
            avg_tfms = []
            while j < n_avg:
                blueprint("Averaging transform %i out of %i." % (j, n_avg))
                ar_tfms = self.cameras.get_ar_markers();
                hyd_tfms = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=None);
                curr_angle = gmt.get_pot_angle(self.lr)
                
                if not ar_tfms and not hyd_tfms:
                    yellowprint("Could not find any transform.")
                    j_th += 1
                    if j_th < thresh:
                        continue
                    else:
                        found = False
                        break
                                
                tfms = ar_tfms
                pot_angle += curr_angle
                tfms.update(hyd_tfms)

                avg_tfms.append(tfms)
                j += 1
                sleeper.sleep()
            
            if found is False:
                yellowprint("Something went wrong; try again.")
                continue
            
            tfms_found = {}
            for tfms in avg_tfms:
                for m in tfms:
                    if m not in tfms_found:
                        tfms_found[m] = []
                    tfms_found[m].append(tfms[m])

            if marker not in tfms_found:
                yellowprint("Could not find marker to be added; try again.")
                continue
            
            for m in tfms_found:
                tfms_found[m] = utils.avg_transform(tfms_found[m])
            pot_angle = pot_angle / n_avg
            
            all_obs.append({'tfms':tfms_found, 'pot_angle':pot_angle})
            i += 1
            
        return all_obs
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)
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        sleeper = rospy.Rate(30)
        
        avg_tfms = {}
        pot_avg = 0.0
        
        j = 0
        n_attempts_max = n_avg*2
        n_attempts_effective = 0;
        while j < n_attempts_max:
            j += 1
            tfms = {}
            # F**k the frames bullshit
            ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers, camera=1)
            hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
            pot_angle = gmt.get_pot_angle(self.lr)
            
            if not ar_tfm or (not hyd_tfm and self.hydras):
                if not ar_tfm:
                    yellowprint('Could not find required ar markers from '+str(self.ar_markers))
                else:
                    yellowprint('Could not find required hydra transforms from '+str(self.hydras))
                continue

            pot_avg += pot_angle
            tfms.update(ar_tfm)
            tfms.update(hyd_tfm)
            
            
            blueprint('\tGetting averaging transform: %d of %d ...'%(n_attempts_effective, n_avg-1))
            n_attempts_effective += 1

            # The angle relevant for each finger is only half the angle.                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
                
            if n_attempts_effective == n_avg:
                break
            
            sleeper.sleep()
            
        if n_attempts_effective < n_avg:
            return False;
            
        pot_avg /= n_avg
        
        greenprint("Average angle found: %f"%pot_avg)

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])
        
        ## only one marker on gripper now
        #if len(avg_tfms) == 1:
        #    yellowprint('Found %i marker only. Not enough to update.'%avg_tfms.keys()[0])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
        return True
def get_transforms(arm, 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 left/ right arm of the pr2 and 
    the hydra paddle of the HYDRA ('right' or 'left') side.
    
    Return gripper_tfms, hydra_tfms, kinect_tfms, head_tfms
    """

    pr2_frame = 'base_footprint'
    assert arm == 'right' or 'left'
    arm_frame = '%s_gripper_tool_frame' % {'right':'r', 'left':'l'}[arm]
    head_frame = 'head_plate_frame'
    hydra_frame = 'hydra_base'
    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
    
    gripper_tfms = []
    hydra_tfms = []
    kinect_tfms = []
    head_tfms = []
    ar_markers = ARMarkersRos('/camera1_')
    time.sleep(3)
    
    
    marker_id = 11
    i = 0
    while i < n_tfm:
        raw_input(colorize("Transform %d of %d : Press return when ready to capture transforms"%(i, n_tfm-1), "red", True))
       
        ## transforms which need to be averaged.
        gtfms = []
        htfms = []
        ktfms = []
        ptfms = []

        sleeper = rospy.Rate(30)  
        collect_enough_data = True      
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j, n_avg-1), "blue", True)
            
            kinect_tfm = ar_markers.get_marker_transforms(time_thresh=0.5)
            print kinect_tfm
            if kinect_tfm == {}:
                print "Lost sight of AR marker. Breaking..."
                collect_enough_data = False
                break
            
            ptrans, prot = tf_sub.lookupTransform(pr2_frame, head_frame, rospy.Time(0))
            ptfms.append(conversions.trans_rot_to_hmat(ptrans,prot))   
            gtrans, grot = tf_sub.lookupTransform(pr2_frame, arm_frame, rospy.Time(0))
            gtfms.append(conversions.trans_rot_to_hmat(gtrans, grot))
            htrans, hrot = tf_sub.lookupTransform(hydra_frame, paddle_frame, rospy.Time(0))
            htfms.append(conversions.trans_rot_to_hmat(htrans, hrot))
            ktrans, krot = conversions.hmat_to_trans_rot(kinect_tfm[marker_id])
            ktfms.append(conversions.trans_rot_to_hmat(ktrans, krot))
            sleeper.sleep()
            
        if collect_enough_data:
            gripper_tfms.append(avg_transform(gtfms))
            hydra_tfms.append(avg_transform(htfms))
            kinect_tfms.append(avg_transform(ktfms))
            head_tfms.append(avg_transform(ptfms))
            
            i += 1

    return (gripper_tfms, hydra_tfms, kinect_tfms, head_tfms)
Ejemplo n.º 29
0
    def add_marker (self, marker, group, m_type='AR', n_tfm=10, n_avg=30):
        """
        Add marker to the gripper, after calibration.
        
        @marker: marker id (a number for AR markers, either 'left' or 'right' for hydras)
        @group: 'l','r' or 'master'
        @m_type: type of marker - 'AR' or 'hydra'
        @n_tfm,@n_avg - the usual 
        """
        # Checks to see if data + situation is valid to add marker
        if marker in self.allmarkers:
            greenprint("Marker already added.")
            return

        if self.cameras is None:
            redprint("Sorry, have no cameras to check.")
            return
        
        if group not in ['master', 'l', 'r']:
            redprint('Invalid group %s' % group)
            return
        
        if m_type not in ['AR', 'hydra']:
            redprint('Invalid marker type %s' % m_type)
            return

        # Get observations 
        all_obs = self.get_obs_new_marker(marker, n_tfm, n_avg)
        
        # Compute average relative transform between correct primary node and marker
        primary_node = self.transform_graph.node[group]['primary']
        avg_rel_tfms = []
        
        for obs in all_obs:
            tfms = obs['tfms']
            angle = obs['pot_angle']
            
            marker_tfm = tfms[marker]
            primary_tfm = self.get_markers_transform([primary_node], tfms, angle)[primary_node]
            
            avg_rel_tfms.append(nlg.inv(primary_tfm).dot(marker_tfm))
            
        rel_tfm = utils.avg_transform(avg_rel_tfms)
        
        # Add markers to relevant lists
        self.transform_graph.node[group]['markers'].append(marker)
        self.allmarkers.append(marker)
        
        if m_type == 'AR':
            self.transform_graph.node[group]['ar_markers'].append(marker)
            self.ar_markers.append(marker)
        else:
            self.transform_graph.node[group]['hydras'].append(marker)
            self.hydra_markers.append(marker)
            
        if group == 'master':
            self.mmarkers.append(marker)
        elif group == 'l':
            self.lmarkers.append(marker)
        else:
            self.rmarkers.append(marker)

        # Update graphs from transform found
        graph = self.transform_graph.node[group]['graph']
        graph.add_node(marker)
        
        for node in graph.nodes_iter():
            graph.add_edge(node, marker)
            graph.add_edge(marker, node)
            
            tfm = graph.edge[node][primary_node]['tfm'].dot(rel_tfm)
            
            graph.edge[node][marker]['tfm'] = tfm
            graph.edge[marker][node]['tfm'] = nlg.inv(tfm)
        
        greenprint("Successfully added your marker to the gripper!")
def optimize_master_transforms (mG, init=None):
    """
    Optimize transforms over the masterGraph (which is a metagraph with nodes as rigid body graphs).
    
    """

    idx = 1
    node_map = {}
    master = None
    for node in mG.nodes_iter():
        if mG.node[node].get("master_marker") is not None:
            master = node 
            node_map[node] = 0
        else:
            node_map[node] = idx
            idx += 1
    
    # Some predefined variables
    I3 = np.eye(3)

    
    def get_mat_from_node (X, node, offset = None):
        """
        Get the matrix from the objective vector depending on the node.
        If offset is specified, return the matrix at the position directly.
        """
        if offset is not None:
            return X[offset:offset+12].reshape([3,4], order='F')

        offset = node_map[node]*12
        Xij = X[offset:offset+12]
        return Xij.reshape([3,4], order='F')
    
    def f_objective (X):
        """
        Objective function to make transforms close to average transforms.
        Sum of the norms of matrix differences between each Tij and 
        """
        obj = 0
        zaxis = np.array([0,0,1])
        
        for g1,g2 in mG.edges_iter():
            for angle in mG.edge[g1][g2]['avg_tfm']:
                t1 = np.r_[get_mat_from_node(X,g1),np.array([[0,0,0,1]])]
                t2 = np.r_[get_mat_from_node(X,g2),np.array([[0,0,0,1]])]

                rad = utils.rad_angle(angle)

                tot_angle = rad*(-mG.node[g1]["angle_scale"]+mG.node[g2]["angle_scale"])
                rot = np.eye(4)
                rot[0:3,0:3] = utils.rotation_matrix(zaxis, tot_angle)
                
                tfm = t1.dot(rot).dot(nlg.inv(t2))

                obj += nlg.norm(tfm - mG.edge[g1][g2]['avg_tfm'][angle])
        
        return obj
    
    def f_constraints (X):
        """
        Constraint function to force matrices to be valid transforms (R.T*R = I_3).
        """
        con = []
        # Constrain Ris to be valid rotations
        
        for node in node_map:
            Ri = get_mat_from_node(X, node)[0:3,0:3]
            con.append(nlg.norm(Ri.T.dot(Ri) - I3))
            
        return np.asarray(con)    
    
    ## Intial value assumes each rigid body has some edge with master.
    ## Averaging out over angles.
    if init is not None:
        x_init = init
    else:
        x_init = np.zeros(12*mG.number_of_nodes())
        x_init[0:9] = I3.reshape(9)
        for node in mG.neighbors_iter(master):
            init_tfm = utils.avg_transform(mG.edge[master][node]['avg_tfm'].values())
            print init_tfm.dot(np.r_[np.c_[np.eye(3),np.array([0,0,0])],np.array([[0,0,0,0]])]).dot(init_tfm.T)
            offset = node_map[node]*12
            x_init[offset:offset+12] = init_tfm[0:3,:].reshape(12, order='F')
        ##

    print "Initial x: ", x_init
    print "Initial objective: ", f_objective(x_init)
    
    (X, fx, _, _, _) = sco.fmin_slsqp(func=f_objective, x0=x_init, f_eqcons=f_constraints, iter=200, full_output=1, iprint=2)
    
    mG_opt = nx.DiGraph()
    ## modify master graph
    node0 = mG.node[master]["primary"]
    
    masterG = mG.node[master]["graph"]
    node_iter = masterG.neighbors(node0) 
    masterG.add_node("cor")
    # change master primary
    mG.node[master]["primary"] = "cor"
    masterG.add_edge(node0,"cor")
    masterG.add_edge("cor",node0)
    masterG.add_edge("cor","cor")
    
    Tsc = np.r_[get_mat_from_node(X, master), np.array([[0,0,0,1]])] 
    masterG.edge[node0]["cor"]['tfm'] = Tsc
    masterG.edge["cor"][node0]['tfm'] = nlg.inv(Tsc)
    masterG.edge["cor"]["cor"]['tfm'] = np.eye(4)
    
    for node in node_iter:
        masterG.add_edge(node,"cor")
        masterG.add_edge("cor",node)
        
        tfm = masterG.edge[node][node0]['tfm'].dot(Tsc)
        masterG.edge[node]["cor"]['tfm'] = tfm
        masterG.edge["cor"][node]['tfm'] = nlg.inv(tfm)
        
    mG_opt.add_node(master)
    mG_opt.node[master]["graph"] = masterG
    mG_opt.node[master]["angle_scale"] = 0
    mG_opt.node[master]["primary"] = "cor"
    mG_opt.node[master]["master_marker"] = mG.node[master]["master_marker"]
    mG_opt.node[master]["markers"] = mG.node[master]["markers"]
    mG_opt.node[master]["hydras"] = mG.node[master]["hydras"]
    mG_opt.node[master]["ar_markers"] = mG.node[master]["ar_markers"]
    ## add edges to the rest
        
    
    for group in mG.nodes_iter():
        if group == master: continue
        mG_opt.add_edge(master, group)
        mG_opt.add_edge(group, master)
        mG_opt.node[group]["graph"] = mG.node[group]["graph"]
        mG_opt.node[group]["angle_scale"] = mG.node[group]["angle_scale"]
        mG_opt.node[group]["primary"] = mG.node[group]["primary"]
        mG_opt.node[group]["hydras"] = mG.node[group]["hydras"]
        mG_opt.node[group]["ar_markers"] = mG.node[group]["ar_markers"]
        
        tfm = np.r_[get_mat_from_node(X, group), np.array([[0,0,0,1]])]
        mG_opt.edge[group][master]['tfm'] = tfm
        mG_opt.edge[master][group]['tfm'] = nlg.inv(tfm)

        tfmFuncs = tfmClass(group,  mG_opt, master, masterG)
        tfmFuncsInv = tfmClassInv(group,  mG_opt, master, masterG)
        mG_opt.edge[master][group]['tfm_func'] = tfmFuncs
        mG_opt.edge[group][master]['tfm_func'] = tfmFuncsInv
        
    
    return mG_opt