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))
def compute_qt_auc_colmap(res_dict, deprecated_images, cfg):
'''Compute pose accuracy (multiview) and add it to the dictionary.'''
qt_acc_list = []
# For all the bags
cfg_bag = deepcopy(cfg)
qt_hist_list = np.empty([0, 10])
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
# Load pose error for colmap
pose_err_dict = load_h5_valid_image(
get_colmap_pose_file(cfg_bag) ,deprecated_images)
# Gather err_q, err_t
err_qt = []
for key, value in pose_err_dict.items():
err_qt += [value]
err_qt = np.asarray(err_qt)
# Take the maximum among q and t errors
err_qt = np.max(err_qt, axis=1)
# Convert to degree
err_qt = err_qt * 180.0 / np.pi
# Make infs to a large value so that np.histogram can be used.
err_qt[err_qt == np.inf] = 1e6
# Create histogram
bars = np.arange(11)
qt_hist, _ = np.histogram(err_qt, bars)
# Normalize histogram with all possible pairs (note that we already
# have error results filled in with infs if necessary, thus we don't
# have to worry about them)
num_pair = float(len(err_qt))
qt_hist = qt_hist.astype(float) / num_pair
# Make cumulative and store to list
qt_acc_list += [np.cumsum(qt_hist)]
qt_hist_list = np.concatenate(
(qt_hist_list, np.expand_dims(qt_hist, axis=0)), axis=0)
# Aggregate all results
qt_acc = np.mean(qt_acc_list, axis=0)
qt_hist = np.squeeze(np.mean(qt_hist_list, axis=0))
# Save to dictionary
res_dict['qt_colmap_01_10'] = qt_hist.tolist()
res_dict['qt_auc_colmap_05'] = np.mean(qt_acc[:5])
res_dict['qt_auc_colmap_10'] = np.mean(qt_acc)
def compute_num_input_matches(res_dict, deprecated_images, cfg):
'''Save the number of input matches given to Colmap.'''
# TODO fix this after re-implementing custom matches
# if cfg.method_dict['config_{}_{}'.format(cfg.dataset,
# cfg.task)]['use_custom_matches']:
# raise NotImplementedError(
# 'TODO Load the right dict with custom matches')
# Read match dict
matches_dict = load_h5_valid_image(
get_filter_match_file(cfg) ,deprecated_images)
# For every bag, compute the number of matches going into colmap
bag_size_json = load_json(
getattr(cfg, 'splits_{}_{}'.format(cfg.dataset, cfg.subset)))
bag_size_list = [b['bag_size'] for b in bag_size_json]
bag_size_num = [b['num_in_bag'] for b in bag_size_json]
# Average it per bag size first, then across all bag sizes
num_input_matches = []
for bag_size, cur_bag_size_num in zip(bag_size_list, bag_size_num):
num_input_matches_bagsize = []
for bag_id in range(cur_bag_size_num):
cfg_bag = deepcopy(cfg)
cfg_bag.bag_size = bag_size
cfg_bag.bag_id = bag_id
# Skip if bag contain deprecated images
if not valid_bag(cfg_bag, deprecated_images):
continue
images = get_colmap_image_path_list(cfg_bag)
keys = [os.path.splitext(os.path.basename(im))[0] for im in images]
pairs = []
for i in range(len(keys)):
for j in range(i + 1, len(keys)):
pairs.append('-'.join(
sorted([keys[i], keys[j]], reverse=True)))
for pair in pairs:
num_input_matches_bagsize.append(matches_dict[pair].shape[-1])
num_input_matches.append(np.mean(num_input_matches_bagsize))
res_dict['num_input_matches'] = np.mean(num_input_matches)
def main(cfg):
'''Visualization of colmap points.
Parameters
----------
cfg: Namespace
Configurations for running this part of the code.
'''
bag_size_json = load_json(
getattr(cfg, 'splits_{}_{}'.format(cfg.dataset, cfg.subset)))
bag_size_list = [b['bag_size'] for b in bag_size_json]
bag_size_num = [b['num_in_bag'] for b in bag_size_json]
# Load deprecated images list
deprecated_images_list = load_json(cfg.json_deprecated_images)
if cfg.scene in deprecated_images_list.keys():
deprecated_images = deprecated_images_list[cfg.scene]
else:
deprecated_images = []
# # Do not re-run if files already exist -- off for now
# skip = True
# for _bag_size in bag_size_list:
# cfg_bag = deepcopy(cfg)
# cfg_bag.bag_size = _bag_size
# viz_folder_hq, viz_folder_lq = get_colmap_viz_folder(cfg_bag)
# for _bag_id in range(
# getattr(cfg_bag,
# 'num_viz_colmap_subsets_bagsize{}'.format(_bag_size))):
# if any([
# not os.path.exists(
# os.path.join(
# viz_folder_lq,
# 'colmap-bagsize{:d}-bag{:02d}-image{:02d}.jpg'.
# format(_bag_size, _bag_id, i)))
# for i in range(_bag_size)
# ]):
# skip = False
# break
# if not os.path.exists(
# os.path.join(
# viz_folder_lq,
# 'colmap-bagsize{:d}-bag{:02d}.pcd'.format(
# _bag_size, _bag_id))):
# skip = False
# break
# if skip:
# print(' -- already exists, skipping colmap visualization')
# return
print(' -- Visualizations, multiview: "{}/{}"'.format(
cfg.dataset, cfg.scene))
t_start = time()
# Create results folder if it does not exist
for _bag_size in bag_size_list:
cfg_bag = deepcopy(cfg)
cfg_bag.bag_size = _bag_size
viz_folder_hq, viz_folder_lq = get_colmap_viz_folder(cfg_bag)
if not os.path.exists(viz_folder_hq):
os.makedirs(viz_folder_hq)
if not os.path.exists(viz_folder_lq):
os.makedirs(viz_folder_lq)
# Load keypoints
keypoints_dict = load_h5(get_kp_file(cfg))
# Loop over bag sizes
for _bag_num, _bag_size in zip(bag_size_num, bag_size_list):
cfg_bag = deepcopy(cfg)
cfg_bag.bag_size = _bag_size
num_bags = getattr(
cfg_bag, 'num_viz_colmap_subsets_bagsize{}'.format(_bag_size))
# select valid bag
valid_bag_ids = []
bag_id = 0
while len(valid_bag_ids) < num_bags:
cfg_bag.bag_id = bag_id
if valid_bag(cfg_bag, deprecated_images):
valid_bag_ids.append(bag_id)
bag_id = bag_id + 1
if bag_id == _bag_num:
raise RuntimeError('Ran out of bags to check out')
for _bag_id in valid_bag_ids:
print(
' -- Visualizations, multiview: "{}/{}", bag_size={}, bag {}/{}'
.format(cfg.dataset, cfg.scene, _bag_size, _bag_id + 1,
num_bags))
# Retrieve list of images
cfg_bag.bag_id = _bag_id
images_in_bag = get_colmap_image_path_list(cfg_bag)
# Retrieve reconstruction
colmap_output_path = get_colmap_output_path(cfg_bag)
# is_colmap_valid = os.path.exists(
# os.path.join(colmap_output_path, '0'))
best_index = get_best_colmap_index(cfg_bag)
if best_index != -1:
colmap_images = read_images_binary(
os.path.join(colmap_output_path, str(best_index),
'images.bin'))
for i, image_path in enumerate(images_in_bag):
# Limit to 10 or so, even for bag size 25
if i >= cfg.max_num_images_viz_multiview:
break
# Load image and keypoints
im, _ = load_image(image_path,
use_color_image=True,
crop_center=False,
force_rgb=True)
used = None
key = os.path.splitext(os.path.basename(image_path))[0]
if best_index != -1:
for j in colmap_images:
if key in colmap_images[j].name:
# plot all keypoints
used = colmap_images[j].point3D_ids != -1
break
if used is None:
used = [False] * keypoints_dict[key].shape[0]
used = np.array(used)
fig = plt.figure(figsize=(20, 20))
plt.imshow(im)
plt.plot(keypoints_dict[key][~used, 0],
keypoints_dict[key][~used, 1],
'r.',
markersize=12)
plt.plot(keypoints_dict[key][used, 0],
keypoints_dict[key][used, 1],
'b.',
markersize=12)
plt.tight_layout()
plt.axis('off')
# TODO Ideally we would save to pdf
# but it does not work on 16.04, so we do png instead
# https://bugs.launchpad.net/ubuntu/+source/imagemagick/+bug/1796563
viz_folder_hq, viz_folder_lq = get_colmap_viz_folder(cfg_bag)
viz_file_hq = os.path.join(
viz_folder_hq,
'bagsize{:d}-bag{:02d}-image{:02d}.png'.format(
_bag_size, _bag_id, i))
viz_file_lq = os.path.join(
viz_folder_lq,
'bagsize{:d}-bag{:02d}-image{:02d}.jpg'.format(
_bag_size, _bag_id, i))
plt.savefig(viz_file_hq, bbox_inches='tight')
# Convert with imagemagick
os.system('convert -quality 75 -resize \"640>\" {} {}'.format(
viz_file_hq, viz_file_lq))
plt.close()
if best_index != -1:
colmap_points = read_points3d_binary(
os.path.join(colmap_output_path, str(best_index),
'points3D.bin'))
points3d = []
for k in colmap_points:
points3d.append([
colmap_points[k].xyz[0], colmap_points[k].xyz[1],
colmap_points[k].xyz[2]
])
points3d = np.array(points3d)
points3d -= np.median(points3d, axis=0)[None, ...]
points3d /= np.abs(points3d).max() + 1e-6
pcd = os.path.join(
get_colmap_viz_folder(cfg_bag)[0],
'colmap-bagsize{:d}-bag{:02d}.pcd'.format(
_bag_size, _bag_id))
with open(pcd, 'w') as f:
f.write('# .PCD v.7 - Point Cloud Data file format\n')
f.write('VERSION .7\n')
f.write('FIELDS x y z\n')
f.write('SIZE 4 4 4\n')
f.write('TYPE F F F\n')
f.write('COUNT 1 1 1\n')
f.write('WIDTH {}\n'.format(len(colmap_points)))
f.write('HEIGHT 1\n')
f.write('VIEWPOINT 0 0 0 1 0 0 0\n')
f.write('POINTS {}\n'.format(len(colmap_points)))
f.write('DATA ascii\n')
for p in points3d:
f.write('{:.05f} {:.05f} {:.05f}\n'.format(
p[0], p[1], p[2]))
copyfile(
os.path.join(
get_colmap_viz_folder(cfg_bag)[0],
'colmap-bagsize{:d}-bag{:02d}.pcd'.format(
_bag_size, _bag_id)),
os.path.join(
get_colmap_viz_folder(cfg_bag)[1],
'colmap-bagsize{:d}-bag{:02d}.pcd'.format(
_bag_size, _bag_id)))
print('done [{:.02f} s.]'.format(time() - t_start))