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
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
# 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
