def compute_ATE(res_dict, deprecated_images, cfg):
'''Compute the Absolute Trajectory Error and add it to the dictionary.'''
ate_list = []
# For all the bags
cfg_bag = deepcopy(cfg)
data_dir = get_data_path(cfg)
calib_list = get_fullpath_list(data_dir, 'calibration')
calib_dict = load_calib(calib_list)
for bag_id in range(get_num_in_bag(cfg_bag)):
cfg_bag.bag_id = bag_id
# Skip if bag contains deprecated images
if not valid_bag(cfg_bag, deprecated_images):
continue
# Get colmap output (binary files) path
colmap_res_path = os.path.join(get_colmap_output_path(cfg_bag),
str(get_best_colmap_index(cfg_bag)))
# Check if colmap output path exists. We compute stats for track
# length, num_cameras, num 3d points, only when colmap succeeds
if os.path.exists(colmap_res_path):
# Read colmap models
_, images, _ = read_model(path=colmap_res_path, ext='.bin')
first_xyz, second_xyz = [], []
for _, value in images.items():
# Get ground truth translation
t_gt = calib_dict[value.name.split('.')[0]]['T']
r_gt = calib_dict[value.name.split('.')[0]]['R']
first_xyz.append(-np.dot(r_gt.T, t_gt))
# Get actual translation
t = np.asarray(value.tvec).flatten().reshape((3, ))
r = np.asarray(qvec2rotmat(value.qvec))
second_xyz.append(-np.dot(r.T, t))
first_xyz = np.asarray(first_xyz).transpose()
second_xyz = np.asarray(second_xyz).transpose()
num_points = first_xyz.shape[1]
if num_points >= 3:
prob_inlier = max(3 / num_points, 0.5)
num_iter = int(calc_num_iter_ransac(prob_inlier))
std = calc_std(first_xyz)
max_trans_error = calc_max_trans_error(first_xyz)
rot, trans, scale, trans_error, num_inlier = ate_ransac(
second_xyz, first_xyz, num_iter, std * 0.5)
if trans_error > max_trans_error or \
num_inlier < 0.3 * num_points:
trans_error = max_trans_error
ate_list += [trans_error]
# Aggregate results for all bags
res_dict['avg_ate'] = float(np.mean(ate_list))
def compute_stereo_metrics_from_colmap(img1, img2, calib1, calib2, best_index,
cfg):
'''Computes (pairwise) error metrics from Colmap results.'''
# Load COLMAP dR and dt
colmap_output_path = get_colmap_output_path(cfg)
# First read images.bin for the best reconstruction
images_bin = read_images_binary(
os.path.join(colmap_output_path, str(best_index), 'images.bin'))
# For each key check if images_bin[key].name = image_name
R_1_actual, t_1_actual = None, None
R_2_actual, t_2_actual = None, None
for key in images_bin.keys():
if images_bin[key].name == os.path.basename(img1):
R_1_actual = qvec2rotmat(images_bin[key].qvec)
t_1_actual = images_bin[key].tvec
if images_bin[key].name == os.path.basename(img2):
R_2_actual = qvec2rotmat(images_bin[key].qvec)
t_2_actual = images_bin[key].tvec
# Compute err_q and err_t only when R, t are not None
err_q, err_t = np.inf, np.inf
if (R_1_actual is not None) and (R_2_actual is not None) and (
t_1_actual is not None) and (t_2_actual is not None):
# Compute dR, dt (actual)
dR_act = np.dot(R_2_actual, R_1_actual.T)
dt_act = t_2_actual - np.dot(dR_act, t_1_actual)
# Get R, t from calibration information
R_1, t_1 = calib1['R'], calib1['T'].reshape((3, 1))
R_2, t_2 = calib2['R'], calib2['T'].reshape((3, 1))
# Compute ground truth dR, dt
dR = np.dot(R_2, R_1.T)
dt = t_2 - np.dot(dR, t_1)
# Save err_, err_t
err_q, err_t = evaluate_R_t(dR, dt, dR_act, dt_act)
return err_q, err_t