def get_tfm(self, master_node, child_node, angle):
        # angle in degrees
        mtfm = self.masterG.edge[master_node]["cor"]['tfm']
        angle = utils.rad_angle(angle)
        rot = np.eye(4)
        rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*self.childN["angle_scale"])
        ctfm = self.childN["graph"].edge[self.childN["primary"]][child_node]['tfm']

        return mtfm.dot(rot).dot(self.gpTfm).dot(ctfm)
        def get_tfm(master_node, child_node, angle):
            # angle in degrees
            mprimary_node = mG_opt.node[master]["primary"]
            #print masterG.edge[master_node][mprimary_node]
            mtfm = masterG.edge[master_node][mprimary_node]['tfm']
            angle = utils.rad_angle(angle)
            rot = np.eye(4)
            rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*mG_opt.node[group]["angle_scale"])
            cprimary_node = mG_opt.node[group]["primary"]
            ctfm = mG_opt.node[group]["graph"].edge[cprimary_node][child_node]['tfm']

            return mtfm.dot(rot).dot(mG_opt.edge[master][group]['tfm']).dot(ctfm)
    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 test_gc2 (n, add_noise=False):
    ##
    # Markers 1,2,3 are on l finger
    # Markers 4,5,6 are on r finger
    # 7 and 8 are on the middle
    ##
    
    ### Initializing everything
    # create calib info
    calib_info = {"master": {"ar_markers":[7,8],
                              "master_group":True,
                              "angle_scale":0},
                  "l": {"ar_markers":[1,2,3],
                        "angle_scale":1},
                  "r": {"ar_markers":[4,5,6],
                        "angle_scale":-1}}
    
    masterGraph = nx.DiGraph()
    
    # Setup the groups
    for group in calib_info:
        masterGraph.add_node(group)
        masterGraph.node[group]["graph"] = nx.Graph()
        masterGraph.node[group]["angle_scale"] = calib_info[group]['angle_scale']
        
        if calib_info[group].get("ar_markers") is None:
            calib_info[group]["ar_markers"] = []
        if calib_info[group].get("hydras") is None:
            calib_info[group]["hydras"] = []
        if calib_info[group].get("ps_markers") is None:
            calib_info[group]["ps_markers"] = []
        
        if calib_info[group].get("master_group") is not None:
            masterGraph.node[group]["master"] = 1
            masterGraph.node[group]['angle_scale'] = 0

        masterGraph.node[group]["markers"] = calib_info[group]["ar_markers"] + calib_info[group]["hydras"] + calib_info[group]["ps_markers"]
        masterGraph.node[group]["calib_info"] = calib_info[group]
    ### Done initializing.
    
    T12 = make_obs()
    T23 = make_obs()
    
    T45 = make_obs()
    T56 = make_obs()
    
    T78 = make_obs()
    
    T7cor = make_obs()
    Tcor1 = make_obs()
    Tcor4 = make_obs()
    
    zaxis = np.array([0,0,1])
    
    for _ in range(n):
        theta = -15 + np.random.random()*30
        theta2 = utils.rad_angle(theta)

        T7 = make_obs()
        
        R1 = np.eye(4)
        R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
        R4 = np.eye(4)
        R4[0:3,0:3] = rotation_matrix(zaxis,-theta2)
        
        if add_noise: 
            Tcor = T7.dot(T7cor)
            
            noise = make_obs(0.01,0.01)
            T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
            noise = make_obs(0.01,0.01)
            T2 = T1.dot(T12).dot(noise)
            noise = make_obs(0.01,0.01)
            T3 = T2.dot(T23).dot(noise)
            
            noise = make_obs(0.01,0.01)
            T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
            noise = make_obs(0.01,0.01)
            T5 = T4.dot(T45).dot(noise)
            noise = make_obs(0.01,0.01)
            T6 = T5.dot(T56).dot(noise)
            
            noise = make_obs(0.01,0.01)
            T8 = T7.dot(T78).dot(noise)
        else:
            noise = np.eye(4)
            
            Tcor = T7.dot(T7cor)
            
            T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
            T2 = T1.dot(T12).dot(noise)
            T3 = T2.dot(T23).dot(noise)
            
            T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
            T5 = T4.dot(T45).dot(noise)
            T6 = T5.dot(T56).dot(noise)
            
            T8 = T7.dot(T78).dot(noise)
            
        # Randomly sample a few until done maybe?
        tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8}
        
        gc.update_groups_from_observations(masterGraph, tfms, theta)
            
    init = np.zeros(36)
    init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F')
    init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F')
    init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F')
    
    G_opt = gc.compute_relative_transforms(masterGraph, 5)#, init)
    
    
    # 7,8 markers
    group = G_opt.node["master"]
    G = group["graph"]
    print "T78"
    print "Original:"
    print T78
    print "Calculated:"
    print G.edge[7][8]['tfm'], '\n'
    
    # 1,2,3 markers
    group = G_opt.node["l"]
    G = group["graph"]
    print "T12"
    print "Original:"
    print T12
    print "Calculated:"
    print G.edge[1][2]['tfm'], '\n'
    
    print "T23"
    print "Original:"
    print T23
    print "Calculated:"
    print G.edge[2][3]['tfm'], '\n'
    
    print "T31"
    print "Original:"
    print nlg.inv(T12.dot(T23))
    print "Calculated:"
    print G.edge[3][1]['tfm'], '\n'
    
    # 4,5,6 markers
    group = G_opt.node["r"]
    G = group["graph"]
    print "T45"
    print "Original:"
    print T45
    print "Calculated:"
    print G.edge[4][5]['tfm'], '\n'
    
    print "T56"
    print "Original:"
    print T56
    print "Calculated:"
    print G.edge[5][6]['tfm'], '\n'
    
    print "T64"
    print "Original:"
    print nlg.inv(T45.dot(T56))
    print "Calculated:"
    print G.edge[6][4]['tfm'], '\n'
    
    # Let's try random angles:

    theta = -15 + np.random.random()*30
    theta2 = utils.rad_angle(theta)
    R1 = np.eye(4)
    R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
    print "T71"
    print "Original:"
    print T7cor.dot(R1).dot(Tcor1)
    print "Calculated:"
    print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
    print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'
    
    print "Difference between cors"
    T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"]
    print nlg.inv(T7cor).dot(T7cor_calib)
def test_gc3 (n, add_noise=False):
    ##
    # Markers 1,2,3 are on l finger
    # Markers 4,5,6 are on r finger
    # 7 and 8 are on the middle
    ##
    
    ### Initializing everything
    # create calib info
    calib_info = {"master": {"ar_markers":[7,8],
                              "master_group":True,
                              "angle_scale":0},
                  "l": {"ar_markers":[1,2,3],
                        "angle_scale":1},
                  "r": {"ar_markers":[4,5,6],
                        "angle_scale":-1}}
    gripper_calib = gc.GripperCalibrator(None, calib_info=calib_info)
    gripper_calib.initialize_calibration(fake_data=True)
        
    T12 = make_obs()
    T23 = make_obs()
    
    T45 = make_obs()
    T56 = make_obs()
    
    T78 = make_obs()
    
    T7cor = make_obs()
    Tcor1 = make_obs()
    Tcor4 = make_obs()
    
    zaxis = np.array([0,0,1])
    
    for _ in range(n):
        theta = -15 + np.random.random()*30
        theta2 = utils.rad_angle(theta)

        T7 = make_obs()
        
        R1 = np.eye(4)
        R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
        R4 = np.eye(4)
        R4[0:3,0:3] = rotation_matrix(zaxis,-theta2)
        
        if add_noise: 
            Tcor = T7.dot(T7cor)
            
            noise = make_obs(0.01,0.01)
            T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
            noise = make_obs(0.01,0.01)
            T2 = T1.dot(T12).dot(noise)
            noise = make_obs(0.01,0.01)
            T3 = T2.dot(T23).dot(noise)
            
            noise = make_obs(0.01,0.01)
            T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
            noise = make_obs(0.01,0.01)
            T5 = T4.dot(T45).dot(noise)
            noise = make_obs(0.01,0.01)
            T6 = T5.dot(T56).dot(noise)
            
            noise = make_obs(0.01,0.01)
            T8 = T7.dot(T78).dot(noise)
        else:
            noise = np.eye(4)
            
            Tcor = T7.dot(T7cor)
            
            T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
            T2 = T1.dot(T12).dot(noise)
            T3 = T2.dot(T23).dot(noise)
            
            T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
            T5 = T4.dot(T45).dot(noise)
            T6 = T5.dot(T56).dot(noise)
            
            T8 = T7.dot(T78).dot(noise)
            
        # Randomly sample a few until done maybe?
        tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8}
        
        gc.update_groups_from_observations(gripper_calib.masterGraph, tfms, theta)
            
    init = np.zeros(36)
    init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F')
    init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F')
    init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F')
    
    gripper_calib.calibrated = gripper_calib.finish_calibration()
    G_opt = gripper_calib.transform_graph 
    
    
    # 7,8 markers
    group = G_opt.node["master"]
    G = group["graph"]
    print "T78"
    print "Original:"
    print T78
    print "Calculated:"
    print G.edge[7][8]['tfm'], '\n'
    
    # 1,2,3 markers
    group = G_opt.node["l"]
    G = group["graph"]
    print "T12"
    print "Original:"
    print T12
    print "Calculated:"
    print G.edge[1][2]['tfm'], '\n'
    
    print "T23"
    print "Original:"
    print T23
    print "Calculated:"
    print G.edge[2][3]['tfm'], '\n'
    
    print "T31"
    print "Original:"
    print nlg.inv(T12.dot(T23))
    print "Calculated:"
    print G.edge[3][1]['tfm'], '\n'
    
    # 4,5,6 markers
    group = G_opt.node["r"]
    G = group["graph"]
    print "T45"
    print "Original:"
    print T45
    print "Calculated:"
    print G.edge[4][5]['tfm'], '\n'
    
    print "T56"
    print "Original:"
    print T56
    print "Calculated:"
    print G.edge[5][6]['tfm'], '\n'
    
    print "T64"
    print "Original:"
    print nlg.inv(T45.dot(T56))
    print "Calculated:"
    print G.edge[6][4]['tfm'], '\n'
    
    # Let's try random angles:

    theta = -15 + np.random.random()*30
    theta2 = utils.rad_angle(theta)
    R1 = np.eye(4)
    R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
    print "Theta: ",theta
    print "T71"
    print "Original:"
    print T7cor.dot(R1).dot(Tcor1)
    print "Calculated:"
    print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
    print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'
    
    theta = -15 + np.random.random()*30
    theta2 = utils.rad_angle(theta)
    R1 = np.eye(4)
    R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
    print "Theta: ",theta
    print "T71"
    print "Original:"
    print T7cor.dot(R1).dot(Tcor1)
    print "Calculated:"
    print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
    print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'

    
    print "Difference between cors"
    T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"]
    print nlg.inv(T7cor).dot(T7cor_calib)

    return gripper_calib
def view_hydra_demo_on_rviz (demo_type, demo_name, freq, speed, prompt=False, verbose=False):
    """
    Uses hydra_only.data for the segment to quickly visualize the demo.
    @demo_type, @demo_name: demo identification.
    @freq: basically measure of fine-ness of timesteps.
    @speed: how fast to replay demo.
    @prompt: does the user hit enter after each time step?
    """
    demo_dir = osp.join(demo_files_dir, demo_type, demo_name)
    bag_file = osp.join(demo_dir, demo_names.bag_name)
    data_file = osp.join(demo_dir, demo_names.hydra_data_name)
    calib_file = osp.join(demo_dir, demo_names.calib_name)
    with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh)
    
    
    if not osp.isfile(data_file):
        yellowprint("%s does not exist for this demo. Extracting now."%demo_names.hydra_data_name)
        ed.save_hydra_only(demo_type, demo_name)
        
    with open(data_file, 'r') as fh: dat = cp.load(fh)
    
    # get grippers used
    grippers = [key for key in dat.keys() if key in 'lr']

    # data 
    rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'}
    cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs}
    
    tfm_pubs = {}
    hydra_dat = {}
    pot_dat = {}
    
    _, hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed, hydra_only=True)
    _, hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed, hydra_only=True)
    tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values(), freq, speed)

    if rospy.get_name() == "/unnamed":
        rospy.init_node("visualize_demo")


    ## publishers for unfiltered-data:
    for lr in grippers:
        tfm_pubs[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped)

    ## get the point-cloud stream
    pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin,speed=speed,verbose=verbose) for cam in rgbd_dirs}
    pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs}

    cam_tfms  = get_cam_transforms (calib_file, len(cam_types))
    for cam in rgbd_dirs:
        if cam != 1:
            publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam])

    sleeper = rospy.Rate(freq)
    T_far = np.eye(4)
    T_far[0:3,3] = [10,10,10]        
    
    handles = []
    
    dat_snext = {lr:{} for lr in grippers}
    for lr in grippers:
        dat_snext[lr]['h'] = stream_soft_next(hydra_dat[lr])
        dat_snext[lr]['pot'] = stream_soft_next(pot_dat[lr])
    
    prev_ang = {'l': 0, 'r': 0}
    for i in xrange(nsteps):
        if prompt:
            raw_input("Hit enter when ready.")
        if verbose:
            print "Time stamp: ", tmin+(0.0+i*speed)/freq
        
        ## show the point-cloud:
        for cam in pc_strms:
            try:
                pc = pc_strms[cam].next()
                if pc is not None:
                    if verbose:
                        print "pc%i ts:"%cam, pc.header.stamp.to_sec()
                    pc.header.stamp = rospy.Time.now()
                    pc_pubs[cam].publish(pc)
                else:
                    if verbose:
                        print "pc%i ts:"%cam,None
            except StopIteration:
                pass

        ests = {}
        tfms = []
        ang_vals  = []

        for lr in grippers:
            ests[lr] = dat_snext[lr]['h']()

            ang_val = dat_snext[lr]['pot']()
            if ang_val != None and not np.isnan(ang_val):
                prev_ang[lr] = ang_val
                ang_val  = ang_val
            else:
                ang_val = prev_ang[lr]
            ang_val *= 2

            if ests[lr] is None:
                tfms.append(T_far)
            else:
                tfms.append(ests[lr])
            ang_vals.append(rad_angle(ang_val))

        handles = draw_trajectory(cam_frames[1], tfms, color=(1,1,0,1), open_fracs=ang_vals)

        for lr in grippers:
            if ests[lr] is not None:
                tfm_pubs[lr].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(ests[lr]), cam_frames[1]))
            
        sleeper.sleep()
    
    empty_cloud = PointCloud2()
    for cam in pc_pubs: pc_pubs[cam].publish(empty_cloud)
def view_tracking_on_rviz(demo_type, demo_name, tps_model_fname, freq=30.0, speed=1.0, use_smoother=True, prompt=False, verbose=False):
    """
    Visualizes demo after kalman tracking/smoothing on rviz.
    @demo_type, @demo_name: demo identification.
    @freq: basically measure of fine-ness of timesteps.
    @speed: how fast to replay demo.
    @main: which sensor to display the marker for
    @prompt: does the user hit enter after each time step?
    """
    
    demo_dir = osp.join(demo_files_dir, demo_type, demo_name)
    bag_file = osp.join(demo_dir, demo_names.bag_name)
    traj_file = osp.join(demo_dir, demo_names.traj_name)
    calib_file = osp.join(demo_dir, demo_names.calib_name)
    with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh)
    
        
    if not osp.isfile(traj_file):
        yellowprint("%s does not exist for this demo. Running kalman filter/smoother now with default args."%demo_names.traj_name)
        data_file = osp.join(demo_dir, demo_names.data_name)
        if not osp.isfile(data_file):
            yellowprint("%s does not exist for this demo. Extracting now."%demo_names.data_name)
            ed.save_observations_rgbd(demo_type, demo_name)
        filter_traj(demo_dir, tps_model_fname=tps_model_fname, save_tps=True, do_smooth=True, plot='', block=False)
    with open(traj_file, 'r') as fh: traj = cp.load(fh)
    
    # get grippers used
    grippers = traj.keys()

    if rospy.get_name() == "/unnamed":
        rospy.init_node("visualize_demo")

    # data 
    rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'}
    pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs}
    cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs}
        
    cam_tfms  = get_cam_transforms (calib_file, len(cam_types))
    for cam in rgbd_dirs:
        if cam != 1:
            publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam])

    # Remove segment "done", it is just a single frame
    segs = sorted(traj[grippers[0]].keys())
    if 'done' in segs: segs.remove('done')
    
    sleeper = rospy.Rate(freq)
    T_far = np.eye(4)
    T_far[0:3,3] = [10,10,10]
    
    for seg in segs:
        if prompt:
            raw_input("Press enter for segment %s."%seg)
        else:
            yellowprint("Segment %s beginning."%seg)
            time.sleep(1)
        
        # Initializing data streams:
        traj_strms = {}
        pot_strms = {}
        tfms_key = 'tfms_s' if use_smoother else 'tfms'

        for lr in grippers:
            traj_strms[lr] = streamize(traj[lr][seg][tfms_key], traj[lr][seg]['stamps'], freq, avg_transform, speed=speed)
            # HACK
            pot_strms[lr] = streamize(traj[lr][seg]['pot_angles'][:len(traj[lr][seg]['stamps'])], traj[lr][seg]['stamps'], freq, np.mean, speed=speed)
        
        tmin, tmax, nsteps = relative_time_streams(traj_strms.values() + pot_strms.values(), freq, speed)
        


        pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin, tend=tmax,speed=speed,verbose=verbose) for cam in rgbd_dirs}
    
        prev_ang = {'l': 0, 'r': 0}

        dat_snext = {lr:{} for lr in grippers}
        for lr in grippers:
            dat_snext[lr]['traj'] = stream_soft_next(traj_strms[lr])
            dat_snext[lr]['pot'] = stream_soft_next(pot_strms[lr])
        

        for i in xrange(nsteps):
            if prompt:
                raw_input("Hit enter when ready.")
            if verbose:
                print "Time stamp: ", tmin+(0.0+i*speed)/freq
            
            ## show the point-cloud:
            found_pc = False
            for cam in pc_strms:
                try:
                    pc = pc_strms[cam].next()
                    if pc is not None:
                        if verbose:
                            print "pc%i ts:"%cam, pc.header.stamp.to_sec()
                        pc.header.stamp = rospy.Time.now()
                        pc_pubs[cam].publish(pc)
                        found_pc = True
                    else:
                        if verbose:
                            print "pc%i ts:"%cam,None
                except StopIteration:
                    pass
    
            tfms = []
            ang_vals  = []
    
            for lr in grippers:
		if lr == 'r':
		    continue
                tfm = dat_snext[lr]['traj']()
                
                ang_val = dat_snext[lr]['pot']()
                if ang_val != None and not np.isnan(ang_val):
                    prev_ang[lr] = ang_val
                    ang_val  = ang_val
                else:
                    ang_val = prev_ang[lr]
                ang_val *= 2

                if tfm is None:
                    tfms.append(T_far)
                else:
                    tfms.append(tfm)
                    
                ang_vals.append(rad_angle(ang_val))
    
    
            handles = draw_trajectory(cam_frames[cam], tfms, color=(1,0,0,1), open_fracs=ang_vals)
            
            time.sleep(1.0/freq)
def view_demo_on_rviz(demo_type, demo_name, freq, speed=1.0, main='h', prompt=False, verbose=False):
    """
    Visualizes recorded demo on rviz (without kalman filter/smoother data).
    @demo_type, @demo_name: demo identification.
    @freq: basically measure of fine-ness of timesteps.
    @speed: how fast to replay demo.
    @main: which sensor to display the marker for
    @prompt: does the user hit enter after each time step?
    """
    demo_dir = osp.join(demo_files_dir, demo_type, demo_name)
    bag_file = osp.join(demo_dir, demo_names.bag_name)
    data_file = osp.join(demo_dir, demo_names.data_name)
    calib_file = osp.join(demo_dir, demo_names.calib_name)
    with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh)
    
    if not osp.isfile(data_file):
        yellowprint("%s does not exist for this demo. Extracting now."%demo_names.data_name)
        ed.save_observations_rgbd(demo_type, demo_name)
    with open(data_file, 'r') as fh: dat = cp.load(fh)
    
    # get grippers used
    grippers = [key for key in dat.keys() if key in 'lr']

    # data 
    rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'}
    cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs}
    
    tfm_pubs = {}

    cam_dat = {}
    hydra_dat = {}
    pot_dat = {}
    
    _, cam_dat['l'], hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed)
    _,  cam_dat['r'], hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed)

    all_cam_strms = []
    for lr in 'lr':
        for cam in cam_dat[lr].keys():
            all_cam_strms.append(cam_dat[lr][cam]['stream'])
    tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values() + all_cam_strms, freq, speed)

    if rospy.get_name() == "/unnamed":
        rospy.init_node("visualize_demo")


    ## publishers for unfiltered-data:
    for lr in grippers:
        tfm_pubs[lr] = {}
        for cam in cam_types:
            tfm_pubs[lr][cam] = rospy.Publisher('/%s_ar%i_estimate'%(lr,cam), PoseStamped)
        tfm_pubs[lr]['h'] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped)

    ## get the point-cloud stream
    pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin,speed=speed,verbose=verbose) for cam in rgbd_dirs}
    pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs}

#     import IPython
#     IPython.embed()

    cam_tfms  = get_cam_transforms (calib_file, len(cam_types))
    for cam in rgbd_dirs:
        if cam != 1:
            publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam])

    sleeper = rospy.Rate(freq)
    T_far = np.eye(4)
    T_far[0:3,3] = [10,10,10]        
    
    handles = []
    
    prev_ang = {'l': 0, 'r': 0}
    
    
    dat_snext = {lr:{} for lr in grippers}
    for lr in grippers:
        dat_snext[lr]['h'] = stream_soft_next(hydra_dat[lr])
        dat_snext[lr]['pot'] = stream_soft_next(pot_dat[lr])
        
        for cam in cam_types:
            dat_snext[lr][cam] = stream_soft_next(cam_dat[lr][cam]['stream'])
        
    
    
    for i in xrange(nsteps):
        if prompt:
            raw_input("Hit enter when ready.")
        if verbose:
            print "Time stamp: ", tmin+(0.0+i*speed)/freq
        
        ## show the point-cloud:
        found_pc = False
        for cam in pc_strms:
            try:
                pc = pc_strms[cam].next()
                if pc is not None:
                    if verbose:
                        print "pc%i ts:"%cam, pc.header.stamp.to_sec()
                    pc.header.stamp = rospy.Time.now()
                    pc_pubs[cam].publish(pc)
                    found_pc = True
                else:
                    if verbose:
                        print "pc%i ts:"%cam,None
            except StopIteration:
                pass

        next_est = {lr:{} for lr in grippers}
        tfms = []
        ang_vals  = []
        
        if main != 'h': main = int(main)

        for lr in grippers:
            next_est[lr]['h'] = dat_snext[lr]['h']()
            for cam in cam_types:
                next_est[lr][cam] = dat_snext[lr][cam]()

            ang_val = dat_snext[lr]['pot']()
            if ang_val != None and not np.isnan(ang_val):
                prev_ang[lr] = ang_val
                ang_val  = ang_val
            else:
                ang_val = prev_ang[lr]
            
            ang_val *= 2
            
            tfm = next_est[lr][main]
            if tfm is None:
                tfms.append(T_far)
            else:
                tfms.append(tfm)
            ang_vals.append(rad_angle(ang_val))

        handles = draw_trajectory(cam_frames[1], tfms, color=(1,1,0,1), open_fracs=ang_vals)

        for lr in grippers:
            for m,est in next_est[lr].items():
                if est != None:
                    tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(est), cam_frames[1]))
                else:
                    tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(T_far), cam_frames[1]))
        
        sleeper.sleep()