def compute_pose_error(cfg):
'''
Computes the error using quaternions and translation vector for COLMAP
'''
if os.path.exists(get_colmap_pose_file(cfg)):
print(' -- already exists, skipping COLMAP eval')
return
# Load visiblity and images
image_path_list = get_colmap_image_path_list(cfg)
subset_index = get_colmap_image_subset_index(cfg, image_path_list)
image_name_list = get_item_name_list(image_path_list)
# Load camera information
data_dir = get_data_path(cfg)
calib_list = get_fullpath_list(data_dir, 'calibration')
calib_dict = load_calib(calib_list, subset_index)
# Generate all possible pairs from all images
pair_list = []
for ii in range(len(image_path_list)):
for jj in range(ii + 1, len(image_path_list)):
pair_list.append([ii, jj])
# Check if colmap results exist. Otherwise, this whole bag is a fail.
colmap_output_path = get_colmap_output_path(cfg)
is_colmap_valid = os.path.exists(os.path.join(colmap_output_path, '0'))
if is_colmap_valid:
# Find the best colmap reconstruction
best_index = get_best_colmap_index(cfg)
print('Computing pose errors')
#num_cores = int(multiprocessing.cpu_count() * 0.9)
num_cores = int(len(os.sched_getaffinity(0)) * 0.9)
result = Parallel(n_jobs=num_cores)(
delayed(compute_stereo_metrics_from_colmap)(image_path_list[
pair[0]], image_path_list[pair[1]], calib_dict[image_name_list[
pair[0]]], calib_dict[image_name_list[pair[1]]],
best_index, cfg)
for pair in tqdm(pair_list))
# Collect err_q, err_t from results
err_dict = {}
for _i in range(len(pair_list)):
pair = pair_list[_i]
if is_colmap_valid:
err_q = result[_i][0]
err_t = result[_i][1]
else:
err_q = np.inf
err_t = np.inf
err_dict[image_name_list[pair[0]] + '-' +
image_name_list[pair[1]]] = [err_q, err_t]
# Finally, save packed errors
save_h5(err_dict, get_colmap_pose_file(cfg))
def get_colmap_vis_list(cfg):
'''Gives a list of visibilities in this bag.'''
data_dir = get_data_path(cfg).rstrip('/') + '_{}bag_{:03d}'.format(
cfg.bag_size, cfg.bag_id)
vis_list = get_fullpath_list(data_dir, 'visibility')
return vis_list
def get_colmap_calib_list(cfg):
'''
Gives a list of calibration files in this bag.
'''
data_dir = get_data_path(cfg)
calib_list = get_fullpath_list(data_dir, 'calibration')
return calib_list
def get_colmap_image_subset_index(cfg, image_subset_list):
data_dir = get_data_path(cfg)
images_list = get_fullpath_list(data_dir, 'images')
subset_index = []
for image2 in image_subset_list:
for i, image1 in enumerate(images_list):
if image2 == image1:
subset_index.append(i)
return subset_index
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 main(cfg):
'''Main function to compute model.
Parameters
----------
cfg: Namespace
Configurations for running this part of the code.
'''
if os.path.exists(get_geom_file(cfg)):
print(' -- already exists, skipping model computation')
return
# Get data directory
keypoints_dict = load_h5(get_kp_file(cfg))
# Load keypoints and matches
matches_dict = load_h5(get_filter_match_file_for_computing_model(cfg))
# Feature Matching
print('Computing model')
num_cores = cfg.num_opencv_threads if cfg.num_opencv_threads > 0 else int(
len(os.sched_getaffinity(0)) * 0.9)
# Load camera information
data_dir = get_data_path(cfg)
images_list = get_fullpath_list(data_dir, 'images')
image_names = get_item_name_list(images_list)
calib_list = get_fullpath_list(data_dir, 'calibration')
calib_dict = load_calib(calib_list)
pairs_per_th = get_pairs_per_threshold(data_dir)
# Get data directory
try:
desc_dict = defaultdict(list)
desc_dict = load_h5(get_desc_file(cfg))
for k, v in desc_dict.items():
desc_dict[k] = v
except Exception:
desc_dict = defaultdict(list)
try:
aff_dict = defaultdict(list)
aff_dict1 = load_h5(get_affine_file(cfg))
for k, v in aff_dict1.items():
aff_dict[k] = v
except Exception:
aff_dict = defaultdict(list)
try:
ori_dict = defaultdict(list)
ori_dict1 = load_h5(get_angle_file(cfg))
for k, v in ori_dict1.items():
ori_dict[k] = v
except Exception:
ori_dict = defaultdict(list)
try:
scale_dict = defaultdict(list)
scale_dict1 = load_h5(get_scale_file(cfg))
for k, v in scale_dict1.items():
scale_dict[k] = v
except Exception:
scale_dict = defaultdict(list)
random.shuffle(pairs_per_th['0.0'])
result = Parallel(n_jobs=num_cores)(delayed(compute_model)(
cfg, np.asarray(matches_dict[pair]),
np.asarray(keypoints_dict[pair.split('-')[0]]),
np.asarray(keypoints_dict[pair.split('-')[1]]), calib_dict[pair.split(
'-')[0]], calib_dict[pair.split('-')[1]], images_list[
image_names.index(pair.split('-')[0])], images_list[
image_names.index(pair.split('-')[1])],
np.asarray(scale_dict[pair.split('-')[0]]),
np.asarray(scale_dict[pair.split('-')[1]]),
np.asarray(ori_dict[pair.split('-')[0]]),
np.asarray(ori_dict[pair.split('-')[1]]),
np.asarray(aff_dict[pair.split('-')[0]]),
np.asarray(aff_dict[pair.split('-')[1]]),
np.asarray(desc_dict[pair.split('-')[0]]),
np.asarray(desc_dict[pair.split('-')[1]]))
for pair in tqdm(pairs_per_th['0.0']))
# Make model dictionary
model_dict = {}
inl_dict = {}
timings_list = []
for i, pair in enumerate(pairs_per_th['0.0']):
model_dict[pair] = result[i][0]
inl_dict[pair] = result[i][1]
timings_list.append(result[i][2])
# Check model directory
if not os.path.exists(get_geom_path(cfg)):
os.makedirs(get_geom_path(cfg))
# Finally save packed models
save_h5(model_dict, get_geom_file(cfg))
save_h5(inl_dict, get_geom_inl_file(cfg))
# Save computational cost
save_h5({'cost': np.mean(timings_list)}, get_geom_cost_file(cfg))
print('Geometry cost (averaged over image pairs): {:0.2f} sec'.format(
np.mean(timings_list)))
def main(cfg):
'''Main function to compute features.
Parameters
----------
cfg: Namespace
Configuration
'''
if os.path.exists(get_kp_file(cfg)) and os.path.exists(get_desc_file(cfg)):
print(' -- already exists, skipping feature extraction')
return
# Get data directory
data_dir = get_data_path(cfg)
# Get list of all images and visibility files in the 'set_100'
images_list = get_fullpath_list(data_dir, 'images')
# Also create a list which only contains the image names, so that it can be
# used as keys in the dictionary later
image_names = get_item_name_list(images_list)
# Create folder
save_dir = get_feature_path(cfg)
if not os.path.exists(save_dir):
os.makedirs(save_dir)
# Compute and save keypoints and descriptors
#
# Parallel processing actually slows down stuff, because opencv is already
# using multiple threads. We just simply go through one by one without
# parallel processing for now
print('Extracting Keypoints and Descriptors:')
result = []
for img_path in tqdm(images_list):
result.append(compute_per_img_file(img_path, cfg))
# num_cores = int(multiprocessing.cpu_count() * 0.9)
# print('Extracting Keypoints and Descriptors:')
# result = Parallel(n_jobs=num_cores)(delayed(compute_per_img_file)(
# img_path, cfg) for img_path in tqdm(images_list))
# Save keypoints and descriptors
kp_dict = {}
scale_dict = {}
angle_dict = {}
score_dict = {}
descs_dict = {}
affine_dict = {}
for _i in range(len(image_names)):
assert 'kp' in result[_i], 'Must provide keypoints'
assert 'descs' in result[_i], 'Must provide descriptors'
if 'kp' in result[_i]:
kp_dict[image_names[_i]] = result[_i]['kp']
if 'scale' in result[_i]:
scale_dict[image_names[_i]] = result[_i]['scale']
if 'angle' in result[_i]:
angle_dict[image_names[_i]] = result[_i]['angle']
if 'affine' in result[_i]:
affine_dict[image_names[_i]] = result[_i]['affine']
if 'score' in result[_i]:
score_dict[image_names[_i]] = result[_i]['score']
if 'descs' in result[_i]:
descs_dict[image_names[_i]] = result[_i]['descs']
# Finally, save packed keypoints and descriptors
save_h5(kp_dict, get_kp_file(cfg))
save_h5(scale_dict, get_scale_file(cfg))
save_h5(angle_dict, get_angle_file(cfg))
save_h5(score_dict, get_score_file(cfg))
save_h5(descs_dict, get_desc_file(cfg))
save_h5(affine_dict, get_affine_file(cfg))
def run_colmap_for_bag(cfg):
'''Runs colmap to retrieve poses for each bag'''
# Colmap pose file already exists, skip the session
if os.path.exists(get_colmap_pose_file(cfg)):
print(' -- already exists, skipping COLMAP eval')
return
# Load keypoints and matches
keypoints_dict = load_h5(get_kp_file(cfg))
matches_dict = load_h5(get_filter_match_file(cfg))
print('Running COLMAP on "{}", bagsize {} -- bag {}'.format(
cfg.scene, cfg.bag_size, cfg.bag_id))
# Additional sanity check to account for crash -- in this case colmap temp
# directory can exist. This in an indication that you need to remove
# results and rerun colmap.
colmap_temp_path = get_colmap_temp_path(cfg)
colmap_output_path = get_colmap_output_path(cfg)
if os.path.exists(colmap_temp_path):
print(' -- temp path exists - cleaning up from crash')
rmtree(colmap_temp_path)
if os.path.exists(colmap_output_path):
rmtree(colmap_output_path)
if os.path.exists(get_colmap_pose_file(cfg)):
os.remove(get_colmap_pose_file(cfg))
# Check existance of colmap result and terminate if already exists.
colmap_output_path = get_colmap_output_path(cfg)
if os.path.exists(colmap_output_path):
print(' -- already exists, skipping COLMAP session')
return
# Create output directory
os.makedirs(colmap_output_path)
# Create colmap temporary directory and copy files over. Remove anything
# that might have existed.
colmap_temp_path = get_colmap_temp_path(cfg)
if os.path.exists(colmap_temp_path):
rmtree(colmap_temp_path)
# Make sure old data is gone and create a new temp folder
assert (not os.path.exists(colmap_temp_path))
os.makedirs(colmap_temp_path)
# Create colmap-friendy structures
os.makedirs(os.path.join(colmap_temp_path, 'images'))
os.makedirs(os.path.join(colmap_temp_path, 'features'))
# Get list of all images in this bag
image_subset_list = get_colmap_image_path_list(cfg)
subset_index = get_colmap_image_subset_index(cfg, image_subset_list)
# Copy images
for _src in image_subset_list:
_dst = os.path.join(colmap_temp_path, 'images', os.path.basename(_src))
copyfile(_src, _dst)
# Write features to colmap friendly format
for image_path in image_subset_list:
# Retrieve image name, with and without extension
image_name = os.path.basename(image_path)
image_name_no_ext = os.path.splitext(image_name)[0]
# Read keypoint
keypoints = keypoints_dict[image_name_no_ext]
# Keypoint file to write to
kp_file = os.path.join(colmap_temp_path, 'features',
image_name + '.txt')
# Open a file to write
with open(kp_file, 'w') as f:
# Retieve the number of keypoints
len_keypoints = len(keypoints)
f.write(str(len_keypoints) + ' ' + str(128) + '\n')
for i in range(len_keypoints):
kp = ' '.join(str(k) for k in keypoints[i][:4])
desc = ' '.join(str(0) for d in range(128))
f.write(kp + ' ' + desc + '\n')
# Write matches to colmap friendly format
# Read visibilties
data_dir = get_data_path(cfg)
vis_list = get_fullpath_list(data_dir, 'visibility')
# Load matches and store them to a text file
# TODO: This seems to be done multiple times. Do we need to do this?
print('Generate list of all possible pairs')
pairs = compute_image_pairs(vis_list, len(image_subset_list), cfg.vis_th,
subset_index)
print('{} pairs generated'.format(len(pairs)))
# Write to match file
match_file = os.path.join(colmap_temp_path, 'matches.txt')
with open(match_file, 'w') as f:
for pair in pairs:
image_1_name = os.path.basename(image_subset_list[pair[0]])
image_2_name = os.path.basename(image_subset_list[pair[1]])
image_1_name_no_ext = os.path.splitext(image_1_name)[0]
image_2_name_no_ext = os.path.splitext(image_2_name)[0]
# Load matches
key = '-'.join([image_1_name_no_ext, image_2_name_no_ext])
matches = np.squeeze(matches_dict[key])
# only write when matches are given
if matches.ndim == 2:
f.write(image_1_name + ' ' + image_2_name + '\n')
for _i in range(matches.shape[1]):
f.write(
str(matches[0, _i]) + ' ' + str(matches[1, _i]) + '\n')
f.write('\n')
f.close()
# COLMAP runs -- wrapped in try except to throw errors if subprocess fails
# and then clean up the colmap temp directory
try:
print('COLMAP Feature Import')
cmd = ['colmap', 'feature_importer']
cmd += [
'--database_path',
os.path.join(colmap_output_path, 'databases.db')
]
cmd += ['--image_path', os.path.join(colmap_temp_path, 'images')]
cmd += ['--import_path', os.path.join(colmap_temp_path, 'features')]
colmap_res = subprocess.run(cmd)
if colmap_res.returncode != 0:
raise RuntimeError(' -- COLMAP failed to import features!')
print('COLMAP Match Import')
cmd = ['colmap', 'matches_importer']
cmd += [
'--database_path',
os.path.join(colmap_output_path, 'databases.db')
]
cmd += [
'--match_list_path',
os.path.join(colmap_temp_path, 'matches.txt')
]
cmd += ['--match_type', 'raw']
cmd += ['--SiftMatching.use_gpu', '0']
colmap_res = subprocess.run(cmd)
if colmap_res.returncode != 0:
raise RuntimeError(' -- COLMAP failed to import matches!')
print('COLMAP Mapper')
cmd = ['colmap', 'mapper']
cmd += ['--image_path', os.path.join(colmap_temp_path, 'images')]
cmd += [
'--database_path',
os.path.join(colmap_output_path, 'databases.db')
]
cmd += ['--output_path', colmap_output_path]
cmd += ['--Mapper.min_model_size', str(cfg.colmap_min_model_size)]
colmap_res = subprocess.run(cmd)
if colmap_res.returncode != 0:
raise RuntimeError(' -- COLMAP failed to run mapper!')
# Delete temp directory after working
rmtree(colmap_temp_path)
except Exception as err:
# Remove colmap output path and temp path
rmtree(colmap_temp_path)
rmtree(colmap_output_path)
# Re-throw error
print(err)
raise RuntimeError('Parts of colmap runs returns failed state!')
print('Checking validity of the colmap run just in case')
# Check validity of colmap reconstruction for all of them
is_any_colmap_valid = False
idx_list = [
os.path.join(colmap_output_path, _d)
for _d in os.listdir(colmap_output_path)
if os.path.isdir(os.path.join(colmap_output_path, _d))
]
for idx in idx_list:
colmap_img_file = os.path.join(idx, 'images.bin')
if is_colmap_img_valid(colmap_img_file):
is_any_colmap_valid = True
break
if not is_any_colmap_valid:
print('Error in reading colmap output -- '
'removing colmap output directory')
rmtree(colmap_output_path)
def main(cfg):
'''Main function to compute matches.
Parameters
----------
cfg: Namespace
Configurations for running this part of the code.
'''
# Get data directory
data_dir = get_data_path(cfg)
# Load pre-computed pairs with the new visibility criteria
pairs_per_th = get_pairs_per_threshold(data_dir)
# Check if all files exist
if is_stereo_complete(cfg):
print(' -- already exists, skipping stereo eval')
return
# Load keypoints and matches
keypoints_dict = load_h5(get_kp_file(cfg))
matches_dict = load_h5(get_match_file(cfg))
geom_dict = load_h5(get_geom_file(cfg))
geom_inl_dict = load_h5(get_geom_inl_file(cfg))
filter_matches_dict = load_h5(get_filter_match_file(cfg))
# Load visiblity and images
images_list = get_fullpath_list(data_dir, 'images')
vis_list = get_fullpath_list(data_dir, 'visibility')
if cfg.dataset != 'googleurban':
depth_maps_list = get_fullpath_list(data_dir, 'depth_maps')
image_names = get_item_name_list(images_list)
# Load camera information
calib_list = get_fullpath_list(data_dir, 'calibration')
calib_dict = load_calib(calib_list)
# Generate all possible pairs
print('Generating list of all possible pairs')
pairs = compute_image_pairs(vis_list, len(image_names), cfg.vis_th)
print('Old pairs with the point-based visibility threshold: {} '
'(for compatibility)'.format(len(pairs)))
for k, v in pairs_per_th.items():
print('New pairs at visibility threshold {}: {}'.format(k, len(v)))
# Evaluate each stereo pair in parallel
# Compute it for all pairs (i.e. visibility threshold 0)
print('Compute stereo metrics for all pairs')
#num_cores = int(multiprocessing.cpu_count() * 0.9)
num_cores = int(len(os.sched_getaffinity(0)) * 0.9)
result = Parallel(n_jobs=num_cores)(delayed(compute_stereo_metrics_from_E)(
images_list[image_names.index(pair.split('-')[0])], images_list[
image_names.index(pair.split('-')[1])],
depth_maps_list[image_names.index(pair.split('-')[0])] if cfg.
dataset != 'googleurban' else None, depth_maps_list[image_names.index(
pair.split('-')[1])] if cfg.dataset != 'googleurban' else None,
np.asarray(keypoints_dict[pair.split('-')[0]]),
np.asarray(keypoints_dict[pair.split('-')[1]]), calib_dict[pair.split(
'-')[0]], calib_dict[pair.split('-')
[1]], geom_dict[pair], matches_dict[pair],
filter_matches_dict[pair], geom_inl_dict[pair], cfg)
for pair in tqdm(pairs_per_th['0.0']))
# Convert previous visibility list to strings
old_keys = []
for pair in pairs:
old_keys.append('{}-{}'.format(image_names[pair[0]],
image_names[pair[1]]))
# Extract scores, err_q, err_t from results
all_keys = pairs_per_th['0.0']
err_dict, rep_s_dict = {}, {}
geo_s_dict_pre_match, geo_s_dict_refined_match, \
geo_s_dict_final_match = {}, {}, {}
true_s_dict_pre_match, true_s_dict_refined_match, \
true_s_dict_final_match = {}, {}, {}
for i in range(len(result)):
if all_keys[i] in old_keys:
if result[i][5]:
geo_s_dict_pre_match[
all_keys[i]] = result[i][0][0] if result[i][0] else None
geo_s_dict_refined_match[
all_keys[i]] = result[i][0][1] if result[i][0] else None
geo_s_dict_final_match[
all_keys[i]] = result[i][0][2] if result[i][0] else None
true_s_dict_pre_match[
all_keys[i]] = result[i][1][0] if result[i][1] else None
true_s_dict_refined_match[
all_keys[i]] = result[i][1][1] if result[i][1] else None
true_s_dict_final_match[
all_keys[i]] = result[i][1][2] if result[i][1] else None
err_q = result[i][2]
err_t = result[i][3]
rep_s_dict[all_keys[i]] = result[i][4]
err_dict[all_keys[i]] = [err_q, err_t]
print('Aggregating results for the old visibility constraint: '
'{}/{}'.format(len(geo_s_dict_pre_match), len(result)))
# Repeat with the new visibility threshold
err_dict_th, rep_s_dict_th = {}, {}
geo_s_dict_pre_match_th, geo_s_dict_refined_match_th, \
geo_s_dict_final_match_th = {}, {}, {}
true_s_dict_pre_match_th, true_s_dict_refined_match_th, \
true_s_dict_final_match_th = {}, {}, {}
for th, cur_pairs in pairs_per_th.items():
_err_dict, _rep_s_dict = {}, {}
_geo_s_dict_pre_match, _geo_s_dict_refined_match, \
_geo_s_dict_final_match = {}, {}, {}
_true_s_dict_pre_match, _true_s_dict_refined_match, \
_true_s_dict_final_match = {}, {}, {}
for i in range(len(all_keys)):
if len(cur_pairs) > 0 and all_keys[i] in cur_pairs:
if result[i][5]:
_geo_s_dict_pre_match[all_keys[
i]] = result[i][0][0] if result[i][0] else None
_geo_s_dict_refined_match[all_keys[
i]] = result[i][0][1] if result[i][0] else None
_geo_s_dict_final_match[all_keys[
i]] = result[i][0][2] if result[i][0] else None
_true_s_dict_pre_match[all_keys[
i]] = result[i][1][0] if result[i][1] else None
_true_s_dict_refined_match[all_keys[
i]] = result[i][1][1] if result[i][1] else None
_true_s_dict_final_match[all_keys[
i]] = result[i][1][2] if result[i][1] else None
err_q = result[i][2]
err_t = result[i][3]
_rep_s_dict[
all_keys[i]] = result[i][4] if result[i][4] else None
_err_dict[all_keys[i]] = [err_q, err_t]
geo_s_dict_pre_match_th[th] = _geo_s_dict_pre_match
geo_s_dict_refined_match_th[th] = _geo_s_dict_refined_match
geo_s_dict_final_match_th[th] = _geo_s_dict_final_match
true_s_dict_pre_match_th[th] = _true_s_dict_pre_match
true_s_dict_refined_match_th[th] = _true_s_dict_refined_match
true_s_dict_final_match_th[th] = _true_s_dict_final_match
err_dict_th[th] = _err_dict
rep_s_dict_th[th] = _rep_s_dict
print('Aggregating results for threshold "{}": {}/{}'.format(
th, len(geo_s_dict_pre_match_th[th]), len(result)))
# Create results folder if it does not exist
if not os.path.exists(get_stereo_path(cfg)):
os.makedirs(get_stereo_path(cfg))
# Finally, save packed scores and errors
if cfg.dataset != 'googleurban':
save_h5(geo_s_dict_pre_match, get_stereo_epipolar_pre_match_file(cfg))
save_h5(geo_s_dict_refined_match,
get_stereo_epipolar_refined_match_file(cfg))
save_h5(geo_s_dict_final_match,
get_stereo_epipolar_final_match_file(cfg))
save_h5(true_s_dict_pre_match,
get_stereo_depth_projection_pre_match_file(cfg))
save_h5(true_s_dict_refined_match,
get_stereo_depth_projection_refined_match_file(cfg))
save_h5(true_s_dict_final_match,
get_stereo_depth_projection_final_match_file(cfg))
save_h5(rep_s_dict, get_repeatability_score_file(cfg))
save_h5(err_dict, get_stereo_pose_file(cfg))
for th in pairs_per_th:
if cfg.dataset != 'googleurban':
save_h5(geo_s_dict_pre_match_th[th],
get_stereo_epipolar_pre_match_file(cfg, th))
save_h5(geo_s_dict_refined_match_th[th],
get_stereo_epipolar_refined_match_file(cfg, th))
save_h5(geo_s_dict_final_match_th[th],
get_stereo_epipolar_final_match_file(cfg, th))
save_h5(true_s_dict_pre_match_th[th],
get_stereo_depth_projection_pre_match_file(cfg, th))
save_h5(true_s_dict_refined_match_th[th],
get_stereo_depth_projection_refined_match_file(cfg, th))
save_h5(true_s_dict_final_match_th[th],
get_stereo_depth_projection_final_match_file(cfg, th))
save_h5(rep_s_dict_th[th], get_repeatability_score_file(cfg, th))
save_h5(err_dict_th[th], get_stereo_pose_file(cfg, th))
def make_xy(sfm_cfg):
"""
Messy conveniency function to re-format our data into that expected by the
Context Networks release.
"""
xs = []
ys = []
Rs = []
ts = []
cx1s = []
cy1s = []
f1s = []
cx2s = []
cy2s = []
f2s = []
key_list = []
data_dir = get_data_path(sfm_cfg)
keypoints_dict = load_h5(get_kp_file(sfm_cfg))
match_dict = load_h5(get_match_file(sfm_cfg))
calib_list = get_fullpath_list(data_dir, 'calibration')
calib_dict = load_calib(calib_list)
print('Converting data to a CNe friendly format...')
for image_pair, match_idx_pairs in tqdm(match_dict.items()):
key_list.append(image_pair)
# Get image name and read image
image_1, image_2 = image_pair.split('-')
image1 = cv2.imread(os.path.join(data_dir, 'images', image_1 + '.jpg'))
image2 = cv2.imread(os.path.join(data_dir, 'images', image_2 + '.jpg'))
# Get dR
R_1 = calib_dict[image_1]['R']
R_2 = calib_dict[image_2]['R']
dR = np.dot(R_2, R_1.T)
# Get dt
t_1 = calib_dict[image_1]['T'].reshape((3, 1))
t_2 = calib_dict[image_2]['T'].reshape((3, 1))
dt = t_2 - np.dot(dR, t_1)
# Save R, t for evaluation
Rs += [np.array(dR).reshape(3, 3)]
# normalize t before saving
dtnorm = np.sqrt(np.sum(dt**2))
assert (dtnorm > 1e-5)
dt /= dtnorm
ts += [np.array(dt).flatten()]
# Save img1, center offset, f
# img1s += [image1.transpose(2, 0, 1)]
cx1 = (image1.shape[1] - 1.0) * 0.5
cy1 = (image1.shape[0] - 1.0) * 0.5
f1 = max(image1.shape[1] - 1.0, image1.shape[0] - 1.0)
cx1s += [cx1]
cy1s += [cy1]
f1s += [f1]
# Save img2, center offset, f
# img2s += [image2.transpose(2, 0, 1)]
cx2 = (image2.shape[1] - 1.0) * 0.5
cy2 = (image2.shape[0] - 1.0) * 0.5
f2 = max(image2.shape[1] - 1.0, image2.shape[0] - 1.0)
cx2s += [cx2]
cy2s += [cy2]
f2s += [f2]
# Get key points
kp1 = np.asarray(keypoints_dict[image_1])
kp1 = kp1[:, :2]
kp2 = np.asarray(keypoints_dict[image_2])
kp2 = kp2[:, :2]
# Normalize Key points
kp1 = (kp1 - np.asarray([cx1, cy1]).T) / np.asarray([f1, f1]).T
kp2 = (kp2 - np.asarray([cx2, cy2]).T) / np.asarray([f2, f2]).T
# Shuffle key points based on match index
x1_index = match_idx_pairs[0, :]
x2_index = match_idx_pairs[1, :]
# Get shuffled key points for image 1
x1 = kp1[x1_index, :]
# Assume depth = 1
z = np.ones((x1.shape[0], 1))
# Construct 3D points
y1 = np.concatenate([x1 * z, z], axis=1)
# Project 3D points to image 2
y1p = np.matmul(dR[None], y1[..., None]) + dt[None]
# move back to the canonical plane
x1p = y1p[:, :2, 0] / y1p[:, 2, 0][..., None]
# Get shuffled key points for image 2
x2 = kp2[x2_index, :]
# make xs in NHWC
xs += [
np.concatenate([x1, x2], axis=1).T.reshape(4, 1, -1).transpose(
(1, 2, 0))
]
# Get the geodesic distance using with x1, x2, dR, dt
geod_d = get_sampsons(x1, x2, dR, dt)
# Get *rough* reprojection errors. Note that the depth may be noisy. We
# ended up not using this...
reproj_d = np.sum((x2 - x1p)**2, axis=1)
# add to label list
ys += [np.stack([geod_d, reproj_d], axis=1)]
res_dict = {}
res_dict['xs'] = xs
res_dict['ys'] = ys
res_dict['Rs'] = Rs
res_dict['ts'] = ts
res_dict['cx1s'] = cx1s
res_dict['cy1s'] = cy1s
res_dict['f1s'] = f1s
res_dict['cx2s'] = cx2s
res_dict['cy2s'] = cy2s
res_dict['f2s'] = f2s
return res_dict, key_list
