コード例 #1
0
def get_tfm_of_camera(ind1,ind2):

    rgbd1_dir = '/home/sibi/sandbox/human_demos/hd_data/demos/sibi_demo/camera_#1'
    rgbd2_dir = '/home/sibi/sandbox/human_demos/hd_data/demos/sibi_demo/camera_#2'
    rgbs1fnames, depths1fnames, stamps1 = eu.get_rgbd_names_times(rgbd1_dir)
    rgbs2fnames, depths2fnames, stamps2 = eu.get_rgbd_names_times(rgbd2_dir)

    winname = 'abcd'

    ar1, ar2 = None, None
    rgb1 = cv2.imread(rgbs1fnames[ind1])
    assert rgb1 is not None
    depth1 = cv2.imread(depths1fnames[ind1],2)
    assert depth1 is not None
    
    cv2.imshow(winname,)
    
    xyz1 = clouds.depth_to_xyz(depth1, asus_xtion_pro_f)
    pc1 = ru.xyzrgb2pc(xyz1, rgb1, frame_id='', use_time_now=False)
    ar_tfms1 = get_ar_marker_poses(pc1)
    if ar_tfms1 and 1 in ar_tfms1:
        blueprint("Got markers " + str(ar_tfms1.keys()) + " at time %f"%stamps1[ind1])
        ar1 = ar_tfms1[1]
        
    rgb2 = cv2.imread(rgbs2fnames[ind2])
    assert rgb2 is not None
    depth2 = cv2.imread(depths2fnames[ind2],2)
    assert depth2 is not None
    xyz2 = clouds.depth_to_xyz(depth2, asus_xtion_pro_f)
    pc2 = ru.xyzrgb2pc(xyz2, rgb2, frame_id='', use_time_now=False)
    ar_tfms2 = get_ar_marker_poses(pc2)
    if ar_tfms2 and 1 in ar_tfms2:
        blueprint("Got markers " + str(ar_tfms2.keys()) + " at time %f"%stamps1[ind2])
        ar2 = ar_tfms2[1]

    if ar1 is None or ar2 is None:
        return None
    return ar1.dot(nlg.inv(ar2))
コード例 #2
0
    def __init__(self, rgbd_dir, frame_id, freq, tstart=None, tend=None, delay=0, speed=1.0, verbose=False):
        if rgbd_dir is None:
            self.done = True
            return

        self.rgbd_dir = rgbd_dir
        self.verbose  = verbose
        self.frame_id = frame_id
        self.tstart   = tstart
        self.tend     = tend
        self.delay    = delay

        self.rgbs_fnames, self.depth_fnames, self.stamps = eu.get_rgbd_names_times(rgbd_dir)
        self.index = 0

        self.base_ts = self.get_stamp()

        self.done = False

        self.speed = speed
        self.dt = self.speed/freq
        self.t  = -self.dt-self.delay if self.tstart==None else self.tstart-self.base_ts-self.dt-self.delay       
        self.ts = 0.0
        self.num_seen = 1
コード例 #3
0
def save_observations_rgbd(demo_type, demo_name, save_file=None):
    """
    Extract data from all sensors and store into demo.data file.
    """
    global setCalib
    # Temp file to show that data is already being extracted
    demo_dir        = osp.join(demo_files_dir, demo_type, demo_name)
    with open(osp.join(demo_dir, demo_names.extract_data_temp),'w') as fh: fh.write('Extracting...')
    
    calib_file_path = osp.join(demo_dir, demo_names.calib_name)
    bag_file        = osp.join(demo_dir, demo_names.bag_name)
    
    with open(osp.join(demo_dir, demo_names.camera_types_name)) as fh:
        camera_types = yaml.load(fh)
    with open(osp.join(demo_dir, demo_names.camera_models_name)) as fh:
        camera_models = yaml.load(fh)
    
    num_cameras = len(camera_types)

    video_dirs = {}
    for i in range(1, num_cameras + 1):
        video_dirs[i] = osp.join(demo_dir, demo_names.video_dir%i)

    c_frames = {}
    for i in range(1, num_cameras + 1):
        c_frames[i]= 'camera%i_link'%(i)
    hydra_frame = 'hydra_base'
    
    tfm_c1 = {i:None for i in range (1,num_cameras+1)}
    tfm_c1[1] = np.eye(4)
    tfm_c1_h = None

    with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh)
    bag = rosbag.Bag(bag_file)
    
    for tfm in calib_data['transforms']:
        if tfm['parent'] == c_frames[1] or tfm['parent'] == '/' + c_frames[1]:
            if tfm['child'] == hydra_frame or tfm['child'] == '/' + hydra_frame:
                tfm_c1_h = nlg.inv(tfm_link_rof).dot(tfm['tfm'])
            else:
                for i in range(2, num_cameras+1):
                    if tfm['child'] == c_frames[i] or tfm['child'] == '/' + c_frames[i]:
                        tfm_c1[i] = nlg.inv(tfm_link_rof).dot(tfm['tfm']).dot(tfm_link_rof)

    if tfm_c1_h is None or not all([tfm_c1[s] != None for s in tfm_c1]):
        redprint("Calibration does not have required transforms")
        return

    if not calib_data.get('grippers'):
        redprint("Gripper not found.")
        return

    grippers = {}
    data = {}
    data['T_cam2hbase'] = tfm_c1_h
    for lr,gdata in calib_data['grippers'].items():
        gr = gripper_lite.GripperLite(lr, gdata['ar'], trans_marker_tooltip=gripper_trans_marker_tooltip[lr])
        gr.reset_gripper(lr, gdata['tfms'], gdata['ar'], gdata['hydra'])
        grippers[lr] = gr
        data[lr] ={'hydra':[],
                   'pot_angles':[],
                   'T_tt2hy': gr.get_rel_transform('tool_tip', gr.hydra_marker)}
        ## place-holder for AR marker transforms:
        for i in xrange(num_cameras):
            data[lr]['camera%d'%(i+1)] = []


    winname = 'cam_image'    
    cam_counts = []
    for i in range(1,num_cameras+1):
        if verbose:
            yellowprint('Camera%i'%i)
        if camera_types[i] == "rgbd":
            rgb_fnames, depth_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i])
        else:
            rgb_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i], depth = False)
        cam_count = len(stamps)
        cam_counts.append(cam_count)

        if camera_types[i] == "rgbd":
            for ind in rgb_fnames:
                rgb = cv2.imread(rgb_fnames[ind])
                
                if displayImages:
                    cv2.imshow(winname, rgb)
                    cv2.waitKey(1)

                assert rgb is not None
                depth = cv2.imread(depth_fnames[ind],2)
                assert depth is not None
                xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
                pc = ru.xyzrgb2pc(xyz, rgb, frame_id='', use_time_now=False)
                ar_tfms = get_ar_marker_poses(pc,track=True)
                if ar_tfms:
                    if verbose:
                        blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind])

                    for lr,gr in grippers.items():
                        ar = gr.get_ar_marker() 
                        if ar in ar_tfms:
                            tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar]))
                            data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind]))
        else:
#             if setCalib is None: 
#                 setCalib = rospy.ServiceProxy("setCalibInfo", SetCalibInfo)
#             reqCalib.camera_model = camera_models[i]
#             setCalib(reqCalib)
#             
#             if verbose:
#                 yellowprint("Changed camera calibration parameters to model %s"%camera_models[i])

            for ind in rgb_fnames:                
                rgb = cv.LoadImage(rgb_fnames[ind])
                
                if displayImages:
                    cv.ShowImage(winname, rgb)
                    cv.WaitKey(1)

                assert rgb is not None
                ar_tfms = get_ar_marker_poses(rgb,use_pc_service=False, model = camera_models[i], track=True)

                if ar_tfms:
                    if verbose:
                        blueprint("Got markers " + str(ar_tfms.keys()) + " at time %f"%stamps[ind])
    
                    for lr,gr in grippers.items():
                        ar = gr.get_ar_marker() 
                        if ar in ar_tfms:
                            tt_tfm = gr.get_tooltip_transform(ar, np.asarray(ar_tfms[ar]))
                            data[lr]['camera%i'%i].append((tfm_c1[i].dot(tt_tfm),stamps[ind]))


    found_hydra_data = False
    found_pot_data = False
    # If hydra_only.data file already exists, don't do extra work.
    hydra_data_file        = osp.join(demo_dir, demo_names.hydra_data_name)
    if osp.isfile(hydra_data_file):
        yellowprint ("%s already exists for %s. Getting hydra data from this info."%(demo_names.hydra_data_name,demo_name))
        with open(hydra_data_file,'r') as fh: hdata = cPickle.load(fh)
         
        for lr in hdata:
            if lr in 'lr':
                if hdata[lr].get('hydra'):
                    data[lr]['hydra'] = hdata[lr]['hydra']
                    found_hydra_data = True
                if hdata[lr].get('pot_angles'):
                    data[lr]['pot_angles'] = hdata[lr]['pot_angles']
                    found_pot_data = True


    if not found_hydra_data:
        if verbose:
            yellowprint('Hydra')
        lr_long = {'l':'left','r':'right'}
        for (_, msg, _) in bag.read_messages(topics=['/tf']):
            hyd_tfm = {}
            found = ''
            for tfm in msg.transforms:
                if found in ['lr','rl']:
                    break
                for lr in grippers:
                    if lr in found:
                        continue
                    elif tfm.header.frame_id == '/' + hydra_frame and tfm.child_frame_id == '/hydra_'+lr_long[lr]:
                        t,r = tfm.transform.translation, tfm.transform.rotation
                        trans = (t.x,t.y,t.z)
                        rot = (r.x, r.y, r.z, r.w)
                        hyd_tfm = tfm_c1_h.dot(conversions.trans_rot_to_hmat(trans, rot))
                        stamp = tfm.header.stamp.to_sec()
                        tt_tfm = grippers[lr].get_tooltip_transform(lr_long[lr], hyd_tfm)
                        data[lr]['hydra'].append((tt_tfm, stamp))
                        found += lr
            if found and verbose:
                blueprint("Got hydra readings %s at time %f"%(found,stamp))

    if not found_pot_data:
        if verbose:
            yellowprint('Potentiometer readings')
        for lr in grippers:
            for (_, msg, ts) in bag.read_messages(topics=['/%s_pot_angle'%lr]):
                angle = msg.data
                stamp = ts.to_sec()
                data[lr]['pot_angles'].append((angle, stamp))
                if verbose: 
                    blueprint("Got a %s potentiometer angle of %f at time %f"%(lr,angle, stamp))

    if verbose:
        for lr in 'lr':
            yellowprint("Gripper %s:"%lr)
            for i in range(num_cameras):
                yellowprint("Found %i transforms out of %i rgb/rgbd images from camera%i"%(len(data[lr]['camera%i'%(i+1)]), cam_counts[i], i+1))
            
            yellowprint("Found %i transforms from hydra"%len(data[lr]['hydra']))
            yellowprint("Found %i potentiometer readings"%len(data[lr]['pot_angles']))

    if save_file is None:
        save_file = demo_names.data_name
    save_filename = osp.join(demo_dir, save_file)

    with open(save_filename, 'w') as sfh: cPickle.dump(data, sfh)
    yellowprint("Saved %s."%demo_names.data_name)
    
    os.remove(osp.join(demo_dir, demo_names.extract_data_temp))
コード例 #4
0
def save_init_ar(demo_type, demo_name, ar_marker, save_in_data_dir=True):
    """
    Extracts the initializing ar marker transform and stores it.
    """    
    # Temp file to show that data is already being extracted
    global setCalib
    demo_dir        = osp.join(demo_files_dir, demo_type, demo_name)
    calib_file_path = osp.join(demo_dir, demo_names.calib_name)
    
    with open(osp.join(demo_dir, demo_names.camera_types_name)) as fh:
        camera_types = yaml.load(fh)
    with open(osp.join(demo_dir, demo_names.camera_models_name)) as fh:
        camera_models = yaml.load(fh)
    
    cameras = range(1,len(camera_types)+1)
    c_frames = {}
    for i in cameras:
        c_frames[i]= 'camera%i_link'%(i)

    video_dirs = {}
    for i in cameras:
        video_dirs[i] = osp.join(demo_dir, demo_names.video_dir%i)
    
    tfm_c1 = {i:None for i in cameras}
    tfm_c1[1] = np.eye(4)

    with open(calib_file_path,'r') as fh: calib_data = cPickle.load(fh)

    for tfm in calib_data['transforms']:
        if tfm['parent'] == c_frames[1] or tfm['parent'] == '/' + c_frames[1]:
            for i in cameras:
                if tfm['child'] == c_frames[i] or tfm['child'] == '/' + c_frames[i]:
                    tfm_c1[i] = nlg.inv(tfm_link_rof).dot(tfm['tfm']).dot(tfm_link_rof)

    if not all([tfm_c1[s] != None for s in tfm_c1]):
        redprint("Calibration does not have required transforms")
        return

    # We want the ar marker transform from all the relevant cameras
    data = {'tfms':{i:None for i in cameras}, 'marker':ar_marker}

    for i in cameras:
        if verbose:
            yellowprint('Camera%i'%i)
        if camera_types[i] == "rgbd":
            rgb_fnames, depth_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i])
        else:
            rgb_fnames, stamps = eu.get_rgbd_names_times(video_dirs[i], depth = False)

        all_tfms = []
        cam_count = 0
        if camera_types[i] == "rgbd":
            for ind in rgb_fnames:
                rgb = cv2.imread(rgb_fnames[ind])
                
                if displayImages:
                    cv2.imshow(winname, rgb)
                    cv2.waitKey(1)

                assert rgb is not None
                depth = cv2.imread(depth_fnames[ind],2)
                assert depth is not None
                xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
                pc = ru.xyzrgb2pc(xyz, rgb, frame_id='', use_time_now=False)
                ar_tfms = get_ar_marker_poses(pc,track=True)
                if ar_tfms and ar_marker in ar_tfms:
                    all_tfms.append(tfm_c1[i].dot(ar_tfms[ar_marker]))
                    cam_count += 1
            
        else:
            if setCalib is None: 
                setCalib = rospy.ServiceProxy("setCalibInfo", SetCalibInfo)
            reqCalib.camera_model = camera_models[i]
            setCalib(reqCalib)

            if verbose:
                yellowprint("Changed camera calibration parameters to model %s"%camera_models[i])

            for ind in rgb_fnames:                
                rgb = cv.LoadImage(rgb_fnames[ind])
                
                if displayImages:
                    cv.ShowImage(winname, rgb)
                    cv.WaitKey(1)

                assert rgb is not None
                ar_tfms = get_ar_marker_poses(rgb,use_pc_service=False,track=True)
                if ar_tfms and ar_marker in ar_tfms:
                    all_tfms.append(tfm_c1[i].dot(ar_tfms[ar_marker]))
                    cam_count += 1
        
        if verbose:
            blueprint ("Got %i values for AR Marker %i from camera %i."%(cam_count, ar_marker, i))
        if cam_count > 0:
            data['tfms'][i] = utils.avg_transform(all_tfms)

    save_filename1 = osp.join(demo_dir, save_file)
    with open(save_filename1, 'w') as sfh: cPickle.dump(data, sfh)
    if save_in_data_dir:
        save_filename2 = osp.join(ar_init_dir, ar_init_demo_name)
        with open(save_filename2, 'w') as sfh: cPickle.dump(data, sfh)
    yellowprint("Saved %s."%save_file)