Esempio n. 1
0
def load_dataset(results_dir, cam_delay):
    print('loading dataset in '+results_dir)   
    print('cam_delay = '+str(cam_delay))

    data_gt = open(os.path.join(results_dir, 'groundtruth.txt')).read()
    lines = data_gt.replace(","," ").replace("\t"," ").split("\n") 
    data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
    #data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if i != 1 and v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
    data_gt = [(float(l[0]),l[1:]) for l in data_gt]
    data_gt = dict(data_gt)    
    
    data_es = open(os.path.join(results_dir, 'traj_estimate.txt')).read()
    lines = data_es.replace(","," ").replace("\t"," ").split("\n") 
    data_es = np.array([[np.float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
    data_es = [(float(l[0]),l[1:]) for l in data_es]
    data_es = dict(data_es)
    
    matches = associate.associate(data_gt, data_es, -cam_delay, 0.02)
    p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a,b in matches])
    q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a,b in matches])
    p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches])
    q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a,b in matches])
    t_gt = np.array([np.float(a) for a,b in matches])
    t_es = np.array([np.float(b) for a,b in matches])
  
    # set start time to zero
    start_time = min(t_es[0], t_gt[0])
    t_es -= start_time
    t_gt -= start_time
    return t_es, p_es, q_es, t_gt, p_gt, q_gt
Esempio n. 2
0
def load_dataset(results_dir, cam_delay):
    print ("loading dataset in " + results_dir)
    print ("cam_delay = " + str(cam_delay))

    data_gt = open(os.path.join(results_dir, "groundtruth.txt")).read()
    lines = data_gt.replace(",", " ").replace("\t", " ").split("\n")
    data_gt = np.array(
        [
            [np.float(v.strip()) for i, v in enumerate(line.split(" ")) if v.strip() != ""]
            for line in lines
            if len(line) > 0 and line[0] != "#"
        ]
    )
    # data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if i != 1 and v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
    data_gt = [(float(l[0]), l[1:]) for l in data_gt]
    data_gt = dict(data_gt)

    data_es = open(os.path.join(results_dir, "traj_estimate.txt")).read()
    lines = data_es.replace(",", " ").replace("\t", " ").split("\n")
    data_es = np.array(
        [
            [np.float(v.strip()) for v in line.split(" ") if v.strip() != ""]
            for line in lines
            if len(line) > 0 and line[0] != "#"
        ]
    )
    data_es = [(float(l[0]), l[1:]) for l in data_es]
    data_es = dict(data_es)

    matches = associate.associate(data_gt, data_es, -cam_delay, 0.02)
    p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a, b in matches])
    q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a, b in matches])
    p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a, b in matches])
    q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a, b in matches])
    t_gt = np.array([np.float(a) for a, b in matches])
    t_es = np.array([np.float(b) for a, b in matches])

    # set start time to zero
    start_time = min(t_es[0], t_gt[0])
    t_es -= start_time
    t_gt -= start_time
    return t_es, p_es, q_es, t_gt, p_gt, q_gt
Esempio n. 3
0
# load dataset parameters
params = yaml.load(open(os.path.join(dataset_dir, 'dataset_params.yaml')))

# set trajectory groundtruth and estimate file
traj_groundtruth = os.path.join(dataset_dir, 'groundtruth.txt')
traj_estimate = os.path.join(dataset_dir,'traj_estimate.txt')

# load data
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)

# select matches
offset = -params['cam_delay']
print ('offset = '+str(offset))
matches = associate.associate(data_gt, data_es, offset, 0.02)
#matches = matches[500:]
p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])

# --------------------------------------------------------------------------------
# align Sim3 to get scale
scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_sim3,:], p_es[0:n_align_sim3,:])

#model_aligned = s * R * model + t
#alignment_error = model_aligned - data
#t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0]
  
p_es_aligned = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans