Exemplo n.º 1
0
def relative_time_streams(fname, lr, freq, single_camera):
    """
    return start-end time, number of time samples, and streams for each sensor
    """
    c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname, lr, single_camera)
    dt =1./freq

    if c2_ts.any():
        tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
        tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))
    else:
        tmin = min(np.min(c1_ts), np.min(hy_ts))
        tmax = max(np.max(c1_ts), np.max(hy_ts))

    ## calculate the number of time-steps for the kalman filter.    
    nsteps = int(math.ceil((tmax-tmin)/dt))
    
    ## get rid of absolute time, put the three data-streams on the same time-scale
    c1_ts -= tmin
    c2_ts -= tmin
    hy_ts -= tmin
    
    ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
    ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
    hy_strm  = streamize(hy_tfs, hy_ts, freq, avg_transform)
    
    return tmin, tmax, nsteps, ar1_strm, ar2_strm, hy_strm
Exemplo n.º 2
0
def setup_kalman(fname, lr, freq, single_camera):
    c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname, lr, single_camera)  
    motion_var, ar_var, hydra_var, hydra_vvar = load_covariances()
    
    dt = 1./freq
    
    if c2_ts.any():
        tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
        tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))
    else:
        tmin = min(np.min(c1_ts), np.min(hy_ts))
        tmax = max(np.max(c1_ts), np.max(hy_ts))     
    
    ## calculate the number of time-steps for the kalman filter.    
    nsteps = int(math.ceil((tmax-tmin)/dt))
    
    ## get rid of absolute time, put the three data-streams on the same time-scale
    c1_ts -= tmin
    c2_ts -= tmin
    hy_ts -= tmin
    
    # initialize KF:
    x0, S0 = get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs)
    KF = kalman()
    
    ## ===> assumes that the variance for ar1 and ar2 are the same!!!    
    KF.init_filter(-dt, x0, S0, motion_var, hydra_var, hydra_vvar, ar_var, ar_var)
    
    ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
    ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
    hy_strm  = streamize(hy_tfs, hy_ts, freq, avg_transform)
    
    return (KF, nsteps, tmin, ar1_strm, ar2_strm, hy_strm)
Exemplo n.º 3
0
def setup_kalman(fname, freq=30.):
    c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname)
    motion_var, ar_var, hydra_var = load_covariances()
    dt = 1/freq
    
    tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
    tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))

    ## calculate the number of time-steps for the kalman filter.    
    nsteps = int(math.ceil((tmax-tmin)/dt))
    
    ## get rid of absolute time, put the three data-streams on the same time-scale
    c1_ts -= tmin
    c2_ts -= tmin
    hy_ts -= tmin
    
    # initialize KF:
    x0, S0 = get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs)
    KF     = kalman()
    
    ## ===> assumes that the variance for ar1 and ar2 are the same!!!
    hydra_var_vel = np.zeros((6, 6))
    hydra_var_vel[3:6, 3:6] = hydra_var[3:6, 3:6]
    hydra_var_vel[0:3, 0:3] = 25e-8 * np.eye(3)
    KF.init_filter(0,x0, S0, motion_var, hydra_var_vel, ar_var, ar_var)
    
    
    
    
    ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
    ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
    hy_strm  = streamize(hy_tfs, hy_ts, freq, avg_transform)
    
    return (KF, nsteps, tmin, ar1_strm, ar2_strm, hy_strm)
def load_data(data_file, lr, freq=30.0):

    with open(data_file, 'r') as f:
        dat = cp.load(f)

    demo_dir    = osp.dirname(data_file)
    cam_types   = get_cam_types(demo_dir)
    T_cam2hbase = dat['T_cam2hbase']
    
    cam_tmin = float('inf')
    
    cam_info = {}
    for kname in dat[lr].keys():
        if 'cam' in kname:
            if kname != 'camera1': continue
            tfs = [tt[0] for tt in dat[lr][kname]]
            ts  = [tt[1] for tt in dat[lr][kname]]
            
            #ctype_name = int(kname[-1])
            ## don't append any empty-streams:
            if len(ts) > 0:
                cam_strm = streamize(tfs, ts, freq, avg_transform)#, tstart=-1./freq)
                cam_info[kname] = {'type'   : cam_types[kname],
                                   'stream' : cam_strm}
                
                cam_tmin = min(ts[0], cam_tmin)

    ## hydra data:
    hydra_tfs = [tt[0] for tt in dat[lr]['hydra']]     
    hydra_ts  = np.array([tt[1] for tt in dat[lr]['hydra']])
    
    if abs(cam_tmin - hydra_ts[0]) > 1000:
        hydra_ts += (cam_tmin - hydra_ts[0])
    
    if len(hydra_ts) <= 0:
        redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file)))
        sys.exit(-1)   
    hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform)#, tstart=-1./freq)

    ## potentiometer angles:
    pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']])
    pot_ts   = np.array([tt[1] for tt in dat[lr]['pot_angles']])
    
    print cam_tmin, pot_ts[0]
    if abs(cam_tmin - pot_ts[0]) > 1000:
        pot_ts += (cam_tmin - pot_ts[0])
    
    if len(pot_ts) <= 0:
        redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file)))
        sys.exit(-1)
    pot_strm = streamize(pot_vals, pot_ts, freq, np.mean)#, tstart=-1./freq)
    
    print hydra_ts[0], pot_ts[0]

    return (T_cam2hbase, cam_info, hydra_strm, pot_strm)
def load_data(data_file, lr, freq=30.0, speed=1.0, hydra_only=False):
    """
    Changed slightly from the one in demo_data_prep to include speed.
    """
    with open(data_file, 'r') as f:
        dat = cp.load(f)

    demo_dir    = osp.dirname(data_file)
    with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh)
    T_cam2hbase = dat['T_cam2hbase']

    if not hydra_only:
        cam_info = {}
        for kname in dat[lr].keys():
            if 'cam' in kname:
                tfs = [tt[0] for tt in dat[lr][kname]]
                ts  = [tt[1] for tt in dat[lr][kname]]
                ctype_name = int(kname[-1])
                ## we will add empty stream
                cam_strm = streamize(tfs, ts, freq, avg_transform, speed=speed)#, tstart=-1./freq)
                cam_info[ctype_name] = {'type'   : cam_types[ctype_name],
                                        'stream' : cam_strm}

    ## hydra data:
    hydra_tfs = [tt[0] for tt in dat[lr]['hydra']]     
    hydra_ts  = np.array([tt[1] for tt in dat[lr]['hydra']])

    if len(hydra_ts) <= 0:
        redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file)))
        sys.exit(-1)
    hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform, speed=speed)#, tstart=-1./freq)

    ## potentiometer angles:
    pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']])
    pot_ts   = np.array([tt[1] for tt in dat[lr]['pot_angles']])
    if len(pot_ts) <= 0:
        redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file)))
        sys.exit(-1)
    pot_strm = streamize(pot_vals, pot_ts, freq, np.mean, speed=speed)#, tstart=-1./freq)

    if not hydra_only:
        return (T_cam2hbase, cam_info, hydra_strm, pot_strm)
    else:
        return (T_cam2hbase, hydra_strm, pot_strm)
Exemplo n.º 6
0
def relative_time_streams(fname, freq):
    c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs = load_data(fname)
    dt =1/freq
    
    tmin = min(np.min(c1_ts), np.min(c2_ts), np.min(hy_ts))
    tmax = max(np.max(c1_ts), np.max(c2_ts), np.max(hy_ts))

    ## calculate the number of time-steps for the kalman filter.    
    nsteps = int(math.ceil((tmax-tmin)/dt))
    
    ## get rid of absolute time, put the three data-streams on the same time-scale
    c1_ts -= tmin
    c2_ts -= tmin
    hy_ts -= tmin
    
    ar1_strm = streamize(c1_tfs, c1_ts, freq, avg_transform)
    ar2_strm = streamize(c2_tfs, c2_ts, freq, avg_transform)
    hy_strm  = streamize(hy_tfs, hy_ts, freq, avg_transform)
    
    return tmin, tmax, nsteps, ar1_strm, ar2_strm, hy_strm
def reject_outliers_tf_stream(strm, n_window=10, v_th=[0.35,0.35,0.25]):
    """
    Rejects transforms from a stream of TF based on
    outliers in the x,y or z coordinates.
    
    Rejects based on threshold on the median velocity computed over a window.
    
    STRM : The stream of transforms to filter
    N_WINDOW: The number of neighbors of a data-point to consider
              (note the neighbors can be far away in time, if the stream is sparse)
    V_TH : The threshold velocity. A data-point is considered an outlier, if v > vth
           V_TH is specified for x,y,z
    
    Note: N_WINDOW/2 number of data-points at the beginning and
          the end of the stream are not filtered.
    """
    s_window = int(n_window/2)
    tfs, ts  = strm.get_data()
    N = len(tfs)
    Xs = np.empty((N,3))
    for i in xrange(N):
        Xs[i,:] = tfs[i][0:3,3]
    
    n = N-2*s_window
    
    ## when stream is very short, just return the original stream
    if n <= 0:
        return strm
    
    v = np.empty((n, 3, 2*s_window))
    
    X0 = Xs[s_window:s_window+n,:]
    t0 = ts[s_window:s_window+n]
    shifts  = np.arange(-s_window, s_window+1)
    shifts  = shifts[np.nonzero(shifts)]
    for i,di in enumerate(shifts):
        dx = Xs[s_window+di:s_window+n+di,:] - X0
        dt = ts[s_window+di: s_window+n+di] - t0
        v[:,:,i] = np.abs(dx/dt[:,None])

    v_med   = np.median(v,axis=2)
    inliers = s_window + np.arange(n)[np.all(v_med <= v_th, axis=1)]
    inliers = np.r_[np.arange(s_window), inliers, np.arange(n+s_window,N)]

    tfs_in = []
    for i in inliers:
        tfs_in.append(tfs[i])
    ts_in = ts[inliers]

    return streamize(tfs_in, ts_in, 1./strm.dt, strm.favg, strm.tstart)
def fit_spline_to_tf_stream(strm, new_freq, deg=3):
    """
    Interpolates a stream of transforms using splines, such that 
    there is no "NONE" when sampling the stream at NEW_FREQ frequency.
    
    Returns a stream of transforms.
    """
    tfs, ts = strm.get_data()
    tstart  = strm.get_start_time()
    tmax    = ts[-1]
    
    ndt = 1./new_freq
    new_ts  = np.arange(tstart, tmax+ndt/4., ndt/2.) 

    ## get data in xyzrpy format (6xn) matrix:
    N = len(tfs)
    tf_dat = np.empty((6, N))
    
    tf_dat[0:3,0] = tfs[0][0:3,3]    
    tf_dat[3:6,0] = tfms.euler_from_matrix(tfs[0])
    for i in xrange(1,N):
        now_tf  = tfs[i]
        tf_dat[0:3,i] = now_tf[0:3,3]
        
        prev_rpy  = tf_dat[3:6,i-1]
        now_rpy   = tfms.euler_from_matrix(now_tf)
        now_rpy   = closer_angle(now_rpy, prev_rpy)
        
        tf_dat[3:6,i] = now_rpy

    blueprint("\t fitting spline to data (scipy) ..")
    s = N*.001**2
    (tck, _) = si.splprep(tf_dat, s=s, u=ts, k=deg)

    blueprint("\t evaluating spline at new time-stamps ..")
    interp_xyzrpys = np.r_[si.splev(new_ts, tck)].T

    smooth_tfms = []
    for xyzrpy in interp_xyzrpys:
        tfm = tfms.euler_matrix(*xyzrpy[3:6])
        tfm[0:3,3] = xyzrpy[0:3]
        smooth_tfms.append(tfm)
        
    return streamize(smooth_tfms, new_ts, new_freq, strm.favg, tstart)
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 traj_kalman_lr(data_file, calib_file, lr, freq, use_spline, customized_shift, single_camera):
    '''
    input: data_file
    lr: 'l' or 'r'
    freq
    use_spline
    customized_shift: custimized shift between smoother and filter: to compensate for the lag of smoother
    
    return trajectory data
    '''
    
    _, _, _, ar1_strm, ar2_strm, hy_strm = relative_time_streams(data_file, lr, freq, single_camera)    
    
    ## run kalman filter:
    nsteps, tmin, F_means, S, A, R = run_kalman_filter(data_file, lr, freq, use_spline, True, single_camera)
    ## run kalman smoother:
    S_means, _ = smoother(A, R, F_means, S)
    
    
    X_kf = np.array(F_means)
    X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T

    X_ks = np.array(S_means)
    X_ks = np.reshape(X_ks, (X_ks.shape[0], X_ks.shape[1])).T
    

    # Shifting
    if customized_shift != None:
        shift = customized_shift
    else:
        shift = correlation_shift(X_kf, X_ks)
        

    X_ks = np.roll(X_ks,shift,axis=1)
    X_ks[:,:shift]  = X_ks[:,shift][:,None]
    
    T_filter = state_to_hmat(list(X_ks.T))
    T_smoother = state_to_hmat(list(X_kf.T))
    
    ## load the potentiometer-angle stream:
    pot_data = cp.load(open(data_file))[lr]['pot_angles']
    
    ang_ts   = np.array([tt[1] for tt in pot_data])  ## time-stamps
    ang_vals = [open_frac(tt[0]) for tt in pot_data]  ## angles
    ang_strm = streamize(ang_vals, ang_ts, freq, lambda x : x[-1], tmin)
    
    
    ang_strm_vals = []

    prev_ang = 0
    for i in xrange(nsteps):
        ang_val = soft_next(ang_strm)
        if ang_val != None:
            prev_ang = ang_val
            ang_val  = [ang_val]
        else:
            ang_val = [prev_ang]
        
        ang_strm_vals.append(ang_val)
        
    stamps = []
    for i in xrange(nsteps):
        stamps.append(tmin + i * 1.0 / freq)
    
        
    traj = {"tfms": T_filter, "tfms_s": T_smoother, "pot_angles": ang_strm_vals, "stamps": stamps}
    
    return traj
def rviz_kalman(demo_dir, bag_file, data_file, calib_file, freq, use_rgbd, use_smoother, use_spline, customized_shift, single_camera):
    '''
    For rgbd, data_file and bag_file are redundant
    Otherwise, demo_dir is redundant
    '''
    if use_rgbd:
        bag_file  = osp.join(demo_dir, 'demo.bag')
        rgbd1_dir = osp.join(demo_dir, 'camera_#1')
        rgbd2_dir = osp.join(demo_dir, 'camera_#2')
        data_file = osp.join(demo_dir, 'demo.data') 
        bag = rosbag.Bag(bag_file)
    else:
        bag = rosbag.Bag(bag_file)

    dat = cp.load(open(data_file))
    grippers = dat.keys()

    pub = rospy.Publisher('/point_cloud1', PointCloud2)
    pub2= rospy.Publisher('/point_cloud2', PointCloud2)
    
    c1_tfm_pub = {}
    c2_tfm_pub = {}
    hydra_tfm_pub = {}
    T_filt = {}
    ang_strm = {}
    ar1_strm = {} 
    ar2_strm = {}
    hy_strm = {}
    smooth_hy = {}
    
    ## publishers for unfiltered-data:
    for lr in grippers:
        c1_tfm_pub[lr] = rospy.Publisher('/%s_ar1_estimate'%(lr), PoseStamped)
        c2_tfm_pub[lr] = rospy.Publisher('/%s_ar2_estimate'%(lr), PoseStamped)
        hydra_tfm_pub[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped)
        
        _, _, _, ar1_strm[lr], ar2_strm[lr], hy_strm = relative_time_streams(data_file, lr, freq, single_camera)
         
        ## run the kalman filter:
        nsteps, tmin, F_means, S,A,R = run_kalman_filter(data_file, lr, freq, use_spline, True, single_camera)
        
        ## run the kalman smoother:
        S_means, _ = smoother(A, R, F_means, S)
        
        X_kf = np.array(F_means)
        X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T
        
        X_ks = np.array(S_means)
        X_ks = np.reshape(X_ks, (X_ks.shape[0], X_ks.shape[1])).T


        # Shifting between filter and smoother:
        if customized_shift != None:
            shift = customized_shift
        else:
            shift = correlation_shift(X_kf, X_ks)

        X_ks = np.roll(X_ks,shift,axis=1)
        X_ks[:,:shift]  = X_ks[:,shift][:,None]
        
        if use_smoother:
            T_filt[lr] = state_to_hmat(list(X_ks.T))
        else:
            T_filt[lr] = state_to_hmat(list(X_kf.T))
        
        ## load the potentiometer-angle stream:
        pot_data = cp.load(open(data_file))[lr]['pot_angles']
        
        ang_ts       = np.array([tt[1] for tt in pot_data])  ## time-stamps
        ang_vals     = [tt[0] for tt in pot_data]  ## angles
#         plt.plot(ang_vals)
#         plt.show()
        ang_vals = [0*open_frac(x) for x in ang_vals]

        ang_strm[lr] = streamize(ang_vals, ang_ts, freq, lambda x : x[-1], tmin)
        
        if use_spline:
            smooth_hy[lr] = (t for t in fit_spline_to_stream(hy_strm, nsteps))
        else:
            smooth_hy[lr] = hy_strm

    ## get the point-cloud stream
    cam1_frame_id = '/camera1_rgb_optical_frame'
    cam2_frame_id = '/camera2_rgb_optical_frame'
    
    if use_rgbd:
        pc1_strm = streamize_rgbd_pc(rgbd1_dir, cam1_frame_id, freq, tmin)
        if single_camera:
            pc2_strm = streamize_rgbd_pc(None, cam2_frame_id, freq, tmin)
        else:
            pc2_strm = streamize_rgbd_pc(rgbd2_dir, cam2_frame_id, freq, tmin)
    else:
        pc1_strm = streamize_pc(bag, '/camera1/depth_registered/points', freq, tmin)
        if single_camera:
            pc2_strm = streamize_pc(bag, None, freq, tmin) 
        else:
            pc2_strm = streamize_pc(bag, '/camera2/depth_registered/points', freq, tmin)  
            

    ## get the relative-transforms between the cameras:
    cam_tfm  = get_cam_transform(calib_file)
    publish_static_tfm(cam1_frame_id, cam2_frame_id, cam_tfm)

    ## frame of the filter estimate:
    sleeper = rospy.Rate(freq)
    T_far = np.eye(4)
    T_far[0:3,3] = [10,10,10]        
    
    handles = []
    
    prev_ang = {'l': 0, 'r': 0}
    for i in xrange(nsteps):
        #raw_input("Hit next when ready.")
        print "Kalman ts: ", tmin+(0.0+i)/freq
        
        ## show the point-cloud:
        found_pc = False
        try:
            pc = pc1_strm.next()
            if pc is not None:
                print "pc1 ts:", pc.header.stamp.to_sec()
                pc.header.stamp = rospy.Time.now()
                pub.publish(pc)
                found_pc = True
            else:
                print "pc1 ts:",None
        except StopIteration:
            print "pc1 ts: finished"
            #print "no more point-clouds"
            pass

        try:
            pc2 = pc2_strm.next()
            if pc2 is not None:
                #print "pc2 not none"
                print "pc2 ts:", pc2.header.stamp.to_sec()
                pc2.header.stamp = rospy.Time.now()
                pub2.publish(pc2)
                found_pc = True
            else:
                print "pc2 ts:", None
        except StopIteration:
            print "pc2 ts: finished"
            pass


        ang_vals  = []
        T_filt_lr = []
        for lr in grippers:
            ang_val = soft_next(ang_strm[lr])
            if ang_val != None:
                prev_ang[lr] = ang_val
                ang_val  = ang_val
            else:
                ang_val = prev_ang[lr]

            ang_vals.append(ang_val)
            T_filt_lr.append(T_filt[lr][i])

        handles = draw_trajectory(cam1_frame_id, T_filt_lr, color=(1,1,0,1), open_fracs=ang_vals)

        # draw un-filtered estimates:
        for lr in grippers:
            ar1_est = soft_next(ar1_strm[lr])
            if ar1_est != None:
                c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar1_est), cam1_frame_id))
            else:
                c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
                
            ar2_est = soft_next(ar2_strm[lr])
            if ar2_est != None:
                c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar2_est), cam1_frame_id))
            else:
                c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
            
            hy_est = soft_next(smooth_hy[lr])
            if hy_est != None:
                hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(hy_est), cam1_frame_id))
            else:
                hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id))
        
        sleeper.sleep()
Exemplo n.º 12
0
def run_KF(KF, nsteps, freq, hydra_strm, cam_dat, hydra_covar, cam_covars, do_smooth, plot, plot_title, block):
    """
    Runs the Kalman filter/smoother for NSTEPS using
    {hydra/rgb/rgbd}_covar as the measurement covariances.
    
    HYDRA_STRM   : TF stream from hydra
    CAM_DAT   : Dictionary of camera streams 
    do_smooth: default true
    plot: default true
    """ 
    
    dt = 1./freq
    
    ## place holders for kalman filter's output:
    xs_kf, covars_kf, ts_kf = [KF.x_filt],[KF.S_filt],[KF.t_filt]

    hydra_snext  = stream_soft_next(hydra_strm)

    cam_strms = [cinfo['stream'] for cinfo in cam_dat.values()]
    cam_names = cam_dat.keys()
    cam_types = [cinfo['type'] for cinfo in cam_dat.values()]
    cam_snext = [stream_soft_next(cstrm) for cstrm in cam_strms]
    
    for i in xrange(nsteps):
        KF.register_tf_observation(hydra_snext(), hydra_covar, do_control_update=True)

        for i in xrange(len(cam_names)):
            cam_covar = cam_covars[cam_names[i]]
            KF.register_tf_observation(cam_snext[i](), cam_covar, do_control_update=False)

        xs_kf.append(KF.x_filt)
        covars_kf.append(KF.S_filt)
        ts_kf.append(KF.t_filt)
        
    hydra_strm.reset()
    for cstrm in cam_strms:
        cstrm.reset()
    if do_smooth:

        '''
        # UNCOMMENT BELOW TO RUN KALMAN SMOOTHER:
        A,_  = KF.get_motion_mats(dt)
        R    = get_smoother_motion_covariance(freq)
        xs_smthr, covars_smthr = smoother(A, R, xs_kf, covars_kf)
        '''
        ### do low-pass filtering for smoothing:
        xs_kf_xyz = np.array(xs_kf)[:,0:3] 
        xs_kf_rpy = np.squeeze(np.array(xs_kf)[:,6:9])

        xs_lp_xyz = low_pass(xs_kf_xyz, freq)
        xs_lp_rpy = low_pass(unbreak(xs_kf_rpy), freq)

        xs_smthr  = np.c_[xs_lp_xyz, np.squeeze(np.array(xs_kf))[:,3:6], xs_lp_rpy, np.squeeze(np.array(xs_kf))[:,9:12]].tolist()
        covars_smthr = None

        if 's' in plot:
            kf_strm   = streamize(state_to_hmat(xs_kf), np.array(ts_kf), 1./hydra_strm.dt, hydra_strm.favg)
            sm_strm   = streamize(state_to_hmat(xs_smthr), np.array(ts_kf), 1./hydra_strm.dt, hydra_strm.favg)
            

            plot_tf_streams([kf_strm, sm_strm, hydra_strm]+cam_strms, ['kf', 'smoother', 'hydra']+cam_dat.keys(), styles=['-','-','.']+['.' for i in xrange(len(cam_strms))], title=plot_title, block=block)
            #plot_tf_streams([hydra_strm]+cam_strms, ['hydra']+cam_dat.keys(), block=block)

        return (ts_kf, xs_kf, covars_kf, xs_smthr, covars_smthr)
    else:
        if 's' in plot:
            kf_strm   = streamize(state_to_hmat(xs_kf), np.array(ts_kf), 1./hydra_strm.dt, hydra_strm.favg)
            plot_tf_streams([kf_strm, hydra_strm]+cam_strms, ['kf', 'hydra']+cam_dat.keys(), styles=['-','-','.']+['.' for i in xrange(len(cam_strms))], title=plot_title, block=block)
            #plot_tf_streams([hydra_strm]+cam_strms, ['hydra']+cam_dat.keys(), block=block)
        return (ts_kf, xs_kf, covars_kf, None, None)
Exemplo n.º 13
0
def load_demo_data(demo_dir, freq, rem_outliers, tps_correct, tps_model_fname, plot, block):
    cam_dat = {}
    hydra_dat  = {}
    pot_dat = {}
    dt      = 1./freq
    lr_full   = {'l': 'left', 'r':'right'}

    data_file = osp.join(demo_dir, demo_names.data_name)

    T_cam2hbase, cam_dat['l'], hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq)
    _, cam_dat['r'], hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq)
    
    ## collect all camera streams in a list:
    all_cam_names = set()
    for lr in 'lr':
        for cam in cam_dat[lr].keys():
            all_cam_names.add(cam)


    ## time-align all tf-streams (wrt their respective hydra-streams):
    ## NOTE THE STREAMS ARE MODIFIED IN PLACE (the time-stamps are changed)
    ## and also the tstart
    blueprint("Getting rid of absolute time..")
    all_cam_strms = []
    for lr in 'lr':
        for cam in cam_dat[lr].keys():
            all_cam_strms.append(cam_dat[lr][cam]['stream'])
    tmin, _, _ = relative_time_streams(hydra_dat.values() + pot_dat.values() + all_cam_strms, freq)
    tshift1 = -tmin

    ## remove outliers in the camera streams
    # plot command: o
    if rem_outliers:
        blueprint("Rejecting TF outliers in camera-data..")
        for lr in 'lr':
            for cam in cam_dat[lr].keys():
                strm_in = reject_outliers_tf_stream(cam_dat[lr][cam]['stream'])
                if 'o' in plot:
                    blueprint("\t Plotting outlier rejection..")
                    cam_name = cam+'_'+lr
                    plot_tf_streams([cam_dat[lr][cam]['stream'], strm_in], strm_labels=[cam_name, cam_name+'_in'], styles=['.','-'], block=block)
                cam_dat[lr][cam]['stream'] = strm_in


    ## do time-alignment (with the respective l/r hydra-streams):
    blueprint("Calculating TF stream time-shifts..")
    time_shifts = {}   ## dictionary : maps a camera-name to the time-shift wrt hydra
    for cam in all_cam_names:
        ndat = {'r':-1, 'l':-1}  ## compare whether left/right has more data-points
        for lr in 'lr':
            if cam in cam_dat[lr].keys():
                ndat[lr] = len(cam_dat[lr][cam]['stream'].get_data()[0])
       
        lr_align = ndat.keys()[np.argmax(ndat.values())]

        time_shifts[cam] = dt* align_tf_streams(hydra_dat[lr_align], cam_dat[lr_align][cam]['stream'])

    greenprint("Time-shifts found : %s"%str(time_shifts))
    
    ## time-shift the streams:
    # plot command: t
    all_cam_strms = []
    blueprint("Time-aligning TF streams..")
    for lr in 'lr':
        redprint("\t Alignment for : %s"%lr_full[lr])
        aligned_cam_strms = []
        for cam in cam_dat[lr].keys():
            aligned_cam_strms.append( time_shift_stream(cam_dat[lr][cam]['stream'], time_shifts[cam]) )

        if 't' in plot:
            unaligned_cam_streams = []
            for cam in cam_dat[lr].values():
                unaligned_cam_streams.append(cam['stream'])
            
            blueprint("\t plotting unaligned TF streams...")
            plot_tf_streams(unaligned_cam_streams + [hydra_dat[lr]], cam_dat[lr].keys()+['hydra'], title='UNALIGNED CAMERA-streams (%s)'%lr_full[lr], block=block)
            blueprint("\t plotting aligned TF streams...")
            plot_tf_streams(aligned_cam_strms+[hydra_dat[lr]], cam_dat[lr].keys()+['hydra'], title='ALIGNED CAMERA-streams (%s)'%lr_full[lr], block=block)
            
        for i,cam in enumerate(cam_dat[lr].keys()):
            cam_dat[lr][cam]['stream'] = aligned_cam_strms[i]
            all_cam_strms.append(cam_dat[lr][cam]['stream']) ###### SIBI'S CHANGE


    ## put the aligned-streams again on the same time-scale: 
    blueprint("Re-aligning the TF-streams after TF based time-shifts..")
    tmin, tmax, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values() + all_cam_strms, freq)
    tshift2 = -tmin
    greenprint("TOTAL TIME-SHIFT : (%0.3f + %0.3f) = %0.3f"%(tshift1, tshift2, tshift1+tshift2))

    ## TPS-correct the hydra-data:
    if tps_correct:
        tps_models = {'l':None, 'r':None}
        if tps_model_fname == '':
            blueprint("\t Fitting TPS model to demo data..")
            for lr in 'lr':
                blueprint("\t TPS fitting for %s"%lr_full[lr])
                if 'camera1' not in cam_dat[lr].keys():
                    redprint("\t\t camera1 not in the data for %s gripper -- Cannot do tps-fit. Skipping.."%lr_full[lr])
                    continue

                _, hydra_tfs, cam1_tfs = get_corresponding_data(hydra_dat[lr], cam_dat[lr]['camera1']['stream'])
                f_tps = fit_tps_on_tf_data(hydra_tfs, cam1_tfs, plot=False) ## <<< mayavi plotter has some issues with multiple insantiations..
                T_cam2hbase_train = T_cam2hbase
                tps_models[lr]    = f_tps

                tps_save_fname = osp.join(demo_dir, 'tps_models.cp')
                with open(tps_save_fname, 'w') as f:
                    cp.dump([tps_models, T_cam2hbase], f)
        else:
            blueprint("\t Getting TPS-model from : %s"%tps_model_fname)
            with open(tps_model_fname, 'r') as f:
                tps_models, T_cam2hbase_train = cp.load(f)

        blueprint("TPS-correcting hydra-data..")
        # plot command: p
        for lr in 'lr':
            if tps_models[lr] != None:
                hydra_tfs, hydra_ts  = hydra_dat[lr].get_data()
                hydra_tfs_aligned = correct_hydra(hydra_tfs, T_cam2hbase, tps_models[lr], T_cam2hbase_train)
                hydra_strm_aligned = streamize(hydra_tfs_aligned, hydra_ts, 1./hydra_dat[lr].dt, hydra_dat[lr].favg, hydra_dat[lr].tstart)

                if 'p' in plot:
                    if tps_model_fname!=None:
                        plot_tf_streams([hydra_dat[lr], hydra_strm_aligned], ['hydra-old', 'hydra-corr'], title='hydra-correction %s'%lr_full[lr], block=block)
                    else:
                        plot_tf_streams([hydra_dat[lr], hydra_strm_aligned, cam_dat[lr]['camera1']['stream']], ['hydra-old', 'hydra-corr', 'cam1'], title='hydra-correction', block=block)

                hydra_dat[lr] = hydra_strm_aligned
                
    ## return the data:
    filter_data = {'demo_dir': demo_dir,
                   'nsteps' : nsteps,
                   'tmin'   : tmin,
                   'tmax'   : tmax,
                   'pot_dat': pot_dat,
                   'hydra_dat': hydra_dat,
                   'cam_dat': cam_dat,
                   't_shift': tshift1+tshift2,
                   'cam_shifts': time_shifts}
    return filter_data