def compute_colmap_stats(res_dict, deprecated_images, cfg):
'''Retrieve stats from Colmap and add them to the dictionary.'''
track_length_list = []
num_cameras_list = []
is_fail_list = []
num_3d_pts_list = []
# For all the bags
cfg_bag = deepcopy(cfg)
for bag_id in range(get_num_in_bag(cfg_bag)):
cfg_bag.bag_id = bag_id
# Skip if bag contain deprecated images
if not valid_bag(cfg_bag, deprecated_images):
continue
# Check if colmap output path exists. We compute stats for track
# length, num_cameras, num 3d points, only when colmap succeeds
if not os.path.exists(get_colmap_output_path(cfg_bag)):
# track_length = 0
# num_cameras = 0
is_fail_list += [1]
# num_3d_pts = 0
else:
is_fail_list += [0]
# 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)))
# Read colmap models
cameras, images, points = read_model(path=colmap_res_path,
ext='.bin')
# Track length
num_obs = []
for idx in points:
num_obs.append(len(np.unique(points[idx].image_ids)))
if len(num_obs) == 0:
num_obs = 0
else:
num_obs = np.array(num_obs)
num_obs = num_obs.mean()
track_length_list += [num_obs]
# Number of cameras
num_cameras_list += [len(list(images.keys()))]
# Number of 3D points
num_3d_pts_list += [len(points)]
# Aggregate results for all bags
res_dict['avg_track_length'] = float(
np.mean(track_length_list)) if len(track_length_list) > 0 else 0.0
res_dict['avg_num_cameras'] = float(
np.mean(num_cameras_list)) if len(num_cameras_list) > 0 else 0.0
res_dict['success_rate'] = float(1.0 - np.mean(is_fail_list))
res_dict['avg_num_3d_points'] = float(
np.mean(num_3d_pts_list)) if len(num_3d_pts_list) > 0 else 0.0
res_dict['num_in_bag'] = float(get_num_in_bag(cfg_bag))
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))