def merge_two_reconstructions(r1, r2, config, threshold=1):
"""Merge two reconstructions with common tracks."""
t1, t2 = r1.points, r2.points
common_tracks = list(set(t1) & set(t2))
if len(common_tracks) > 6:
# Estimate similarity transform
p1 = np.array([t1[t].coordinates for t in common_tracks])
p2 = np.array([t2[t].coordinates for t in common_tracks])
T, inliers = multiview.fit_similarity_transform(
p1, p2, max_iterations=1000, threshold=threshold)
if len(inliers) >= 10:
s, A, b = multiview.decompose_similarity_transform(T)
r1p = r1
align.apply_similarity(r1p, s, A, b)
r = r2
r.shots.update(r1p.shots)
r.points.update(r1p.points)
align.align_reconstruction(r, None, config)
return [r]
else:
return [r1, r2]
else:
return [r1, r2]
def merge_two_reconstructions(r1, r2, config, threshold=1):
"""Merge two reconstructions with common tracks."""
t1, t2 = r1.points, r2.points
common_tracks = list(set(t1) & set(t2))
if len(common_tracks) > 6:
# Estimate similarity transform
p1 = np.array([t1[t].coordinates for t in common_tracks])
p2 = np.array([t2[t].coordinates for t in common_tracks])
T, inliers = multiview.fit_similarity_transform(p1,
p2,
max_iterations=1000,
threshold=threshold)
if len(inliers) >= 10:
s, A, b = multiview.decompose_similarity_transform(T)
r1p = r1
align.apply_similarity(r1p, s, A, b)
r = r2
r.shots.update(r1p.shots)
r.points.update(r1p.points)
align.align_reconstruction(r, None, config)
return [r]
else:
return [r1, r2]
else:
return [r1, r2]
def aligned_to_reference(reference, reconstruction):
"""Align a reconstruction to a reference."""
coords1, coords2 = [], []
for point1 in reconstruction.points.values():
point2 = reference.points.get(point1.id)
if point2 is not None:
coords1.append(point1.coordinates)
coords2.append(point2.coordinates)
s, A, b = find_alignment(coords1, coords2)
aligned = _copy_reconstruction(reconstruction)
align.apply_similarity(aligned, s, A, b)
return aligned
def apply_transformations(transformations):
submodels = itertools.groupby(transformations.keys(), lambda key: key.submodel_path)
for submodel_path, keys in submodels:
data = dataset.DataSet(submodel_path)
if not data.reconstruction_exists():
continue
reconstruction = data.load_reconstruction()
for key in keys:
partial_reconstruction = reconstruction[key.index]
s, A, b = transformations[key]
align.apply_similarity(partial_reconstruction, s, A, b)
data.save_reconstruction(reconstruction, 'reconstruction.aligned.json')
def apply_transformations(transformations):
submodels = itertools.groupby(transformations.keys(), lambda key: key.submodel_path)
for submodel_path, keys in submodels:
data = dataset.DataSet(submodel_path)
if not data.reconstruction_exists():
continue
reconstruction = data.load_reconstruction()
for key in keys:
partial_reconstruction = reconstruction[key.index]
s, A, b = transformations[key]
align.apply_similarity(partial_reconstruction, s, A, b)
data.save_reconstruction(reconstruction, 'reconstruction.aligned.json')
def merge_two_reconstructions(r1, r2, config, threshold=1):
"""Merge two reconstructions with common tracks IDs."""
common_tracks = list(set(r1.points) & set(r2.points))
worked, T, inliers = align_two_reconstruction(r1, r2, common_tracks, threshold)
if worked and len(inliers) >= 10:
s, A, b = multiview.decompose_similarity_transform(T)
r1p = r1
apply_similarity(r1p, s, A, b)
r = r2
r.shots.update(r1p.shots)
r.points.update(r1p.points)
align_reconstruction(r, None, config)
return [r]
else:
return [r1, r2]
def merge_two_reconstructions(r1, r2, config, threshold=1):
"""Merge two reconstructions with common tracks IDs."""
common_tracks = list(set(r1.points) & set(r2.points))
worked, T, inliers = align_two_reconstruction(
r1, r2, common_tracks, threshold)
if worked and len(inliers) >= 10:
s, A, b = multiview.decompose_similarity_transform(T)
r1p = r1
apply_similarity(r1p, s, A, b)
r = r2
r.shots.update(r1p.shots)
r.points.update(r1p.points)
align_reconstruction(r, None, config)
return [r]
else:
return [r1, r2]
def aligned_to_reference(
reference: types.Reconstruction, reconstruction: types.Reconstruction
) -> types.Reconstruction:
"""Align a reconstruction to a reference."""
coords1, coords2 = [], []
for point1 in reconstruction.points.values():
point2 = reference.points.get(point1.id)
if point2 is not None:
coords1.append(point1.coordinates)
coords2.append(point2.coordinates)
if len(coords1) == 0 or len(coords2) == 0:
for shot1 in reconstruction.shots.values():
shot2 = reference.shots.get(shot1.id)
if shot2 is not None:
coords1.append(shot1.pose.get_origin())
coords2.append(shot2.pose.get_origin())
s, A, b = find_alignment(coords1, coords2)
aligned = _copy_reconstruction(reconstruction)
align.apply_similarity(aligned, s, A, b)
return aligned
def main():
args = parse_args()
path = args.dataset
data = dataset.DataSet(path)
for fn in ("reconstruction.json", "ground_control_points.json",
"tracks.csv"):
if not (os.path.exists(os.path.join(path, fn))):
logger.error(f"Missing file: {fn}")
return
assert args.rec_a != args.rec_b, "rec_a and rec_b should be different"
camera_models = data.load_camera_models()
tracks_manager = data.load_tracks_manager()
gcps = data.load_ground_control_points()
fn_resplit = f"reconstruction_gcp_ba_resplit_{args.rec_a}x{args.rec_b}.json"
fn_rigid = f"reconstruction_gcp_rigid_{args.rec_a}x{args.rec_b}.json"
if args.rec_b: # reconstruction - to - reconstruction annotation
if args.fast and os.path.exists(data._reconstruction_file(fn_resplit)):
reconstructions = data.load_reconstruction(fn_resplit)
else:
reconstructions = data.load_reconstruction()
reconstructions = [
reconstructions[args.rec_a], reconstructions[args.rec_b]
]
coords0 = triangulate_gcps(gcps, reconstructions[0])
coords1 = triangulate_gcps(gcps, reconstructions[1])
s, A, b = find_alignment(coords1, coords0)
align.apply_similarity(reconstructions[1], s, A, b)
else: # Image - to - reconstruction annotation
reconstructions = data.load_reconstruction()
base = reconstructions[args.rec_a]
resected = resect_annotated_single_images(base, gcps, camera_models,
data)
for shot in resected.shots.values():
shot.metadata.gps_accuracy.value = 1e12
shot.metadata.gps_position.value = shot.pose.get_origin()
reconstructions = [base, resected]
data.save_reconstruction(reconstructions, fn_rigid)
merged = merge_reconstructions(reconstructions, tracks_manager)
# data.save_reconstruction(
# [merged], f"reconstruction_merged_{args.rec_a}x{args.rec_b}.json"
# )
if not args.fast:
data.config["bundle_max_iterations"] = 200
data.config["bundle_use_gcp"] = True
print("Running BA ...")
orec.bundle(merged, camera_models, gcp=gcps, config=data.config)
# rigid rotation to put images on the ground
orec.align_reconstruction(merged, None, data.config)
# data.save_reconstruction(
# [merged], "reconstruction_gcp_ba_{args.rec_a}x{args.rec_b}.json"
# )
gcp_reprojections = reproject_gcps(gcps, merged)
reprojection_errors = get_all_reprojection_errors(gcp_reprojections)
err_values = [t[2] for t in reprojection_errors]
max_reprojection_error = np.max(err_values)
median_reprojection_error = np.median(err_values)
with open(
f"{data.data_path}/gcp_reprojections_{args.rec_a}x{args.rec_b}.json",
"w") as f:
json.dump(gcp_reprojections, f, indent=4, sort_keys=True)
gcp_std = compute_gcp_std(gcp_reprojections)
logger.info(f"GCP reprojection error STD: {gcp_std}")
if not args.fast:
resplit = resplit_reconstruction(merged, reconstructions)
data.save_reconstruction(resplit, fn_resplit)
all_shots_std = []
# We run bundle by fixing one reconstruction.
# If we have two reconstructions, we do this twice, fixing each one.
_rec_ixs = [(0, 1), (1, 0)] if args.rec_b else [(0, 1)]
for rec_ixs in _rec_ixs:
print(f"Running BA with fixed images. Fixing rec #{rec_ixs[0]}")
fixed_images = set(reconstructions[rec_ixs[0]].shots.keys())
bundle_with_fixed_images(
merged,
camera_models,
gcp=gcps,
gcp_std=gcp_std,
fixed_images=fixed_images,
config=data.config,
)
logger.info(
f"STD in the position of shots in R#{rec_ixs[1]} w.r.t R#{rec_ixs[0]}"
)
for shot in merged.shots.values():
if shot.id in reconstructions[rec_ixs[1]].shots:
u, std = decompose_covariance(shot.covariance[3:, 3:])
all_shots_std.append((shot.id, np.linalg.norm(std)))
logger.info(
f"{shot.id} position std: {np.linalg.norm(std)}")
# If the STD of all shots is the same, replace by nan
std_values = [x[1] for x in all_shots_std]
n_bad_std = sum(std > args.std_threshold for std in std_values)
n_good_std = sum(std <= args.std_threshold for std in std_values)
if np.allclose(std_values, std_values[0]):
all_shots_std = [(x[0], np.nan) for x in all_shots_std]
n_bad_std = len(std_values)
# Average positional STD
median_shot_std = np.median([t[1] for t in all_shots_std])
# Save the shot STD to a file
with open(f"{data.data_path}/shots_std_{args.rec_a}x{args.rec_b}.csv",
"w") as f:
s = sorted(all_shots_std, key=lambda t: -t[-1])
for t in s:
line = "{}, {}".format(*t)
f.write(line + "\n")
max_shot_std = s[0][1]
got_shots_std = not np.isnan(all_shots_std[0][1])
else:
n_bad_std = -1
n_good_std = -1
median_shot_std = -1
max_shot_std = -1
got_shots_std = False
n_bad_gcp_annotations = int(
sum(t[2] > args.px_threshold for t in reprojection_errors))
for t in reprojection_errors:
if t[2] > args.px_threshold:
print(t)
metrics = {
"n_reconstructions": len(data.load_reconstruction()),
"median_shot_std": median_shot_std,
"max_shot_std": max_shot_std,
"max_reprojection_error": max_reprojection_error,
"median_reprojection_error": median_reprojection_error,
"n_gcp": len(gcps),
"n_bad_gcp_annotations": n_bad_gcp_annotations,
"n_bad_position_std": int(n_bad_std),
"n_good_position_std": int(n_good_std),
"rec_a": args.rec_a,
"rec_b": args.rec_b,
}
logger.info(metrics)
p_metrics = f"{data.data_path}/run_ba_metrics_{args.rec_a}x{args.rec_b}.json"
with open(p_metrics, "w") as f:
json.dump(metrics, f, indent=4, sort_keys=True)
logger.info(f"Saved metrics to {p_metrics}")
logger.info("========================================")
logger.info("=============== Summary ================")
logger.info("========================================")
if n_bad_std == 0 and n_bad_gcp_annotations == 0:
logger.info(
f"No issues. All gcp reprojections are under {args.px_threshold}"
f" and all frames are localized within {args.std_threshold}m")
if n_bad_std == 0 and n_bad_gcp_annotations == 0:
logger.info(
f"No issues. All gcp reprojections are under {args.px_threshold}"
f" and all frames are localized within {args.std_threshold}m")
if n_bad_std != 0 or n_bad_gcp_annotations != 0:
if args.fast:
logger.info(
"Positional uncertainty unknown since analysis ran in fast mode."
)
elif not got_shots_std:
logger.info(
"Could not get positional uncertainty. It could be because:"
"\na) there are not enough GCPs."
"\nb) they are badly distributed in 3D."
"\nc) there are some wrong annotations")
else:
logger.info(
f"{n_bad_std} badly localized images (error>{args.std_threshold})."
" Use the frame list on each view to find these")
logger.info(
f"{n_bad_gcp_annotations} annotations with large reprojection error."
" Worst offenders:")
stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp(
gcp_reprojections, args.px_threshold)
gcps_sorted = sorted(stats_bad_reprojections,
key=lambda k: -stats_bad_reprojections[k])[:5]
for ix, gcp_id in enumerate(gcps_sorted):
n = stats_bad_reprojections[gcp_id]
if n > 0:
logger.info(f"#{ix+1} - {gcp_id}: {n} bad annotations")
def align(
path: str,
rec_a: int,
rec_b: int,
rigid: bool,
covariance: bool,
px_threshold: float,
std_threshold: float,
):
data = dataset.DataSet(path)
for fn in ("reconstruction.json", "ground_control_points.json", "tracks.csv"):
if not (os.path.exists(os.path.join(path, fn))):
logger.error(f"Missing file: {fn}")
return
camera_models = data.load_camera_models()
tracks_manager = data.load_tracks_manager()
fix_3d_annotations_in_gcp_file(data)
gcps = data.load_ground_control_points()
fn_resplit = f"reconstruction_gcp_ba_resplit_{rec_a}x{rec_b}.json"
fn_rigid = f"reconstruction_gcp_rigid_{rec_a}x{rec_b}.json"
reconstructions = data.load_reconstruction()
if len(reconstructions) > 1:
if rec_b is not None: # reconstruction - to - reconstruction alignment
if rigid and os.path.exists(data._reconstruction_file(fn_resplit)):
reconstructions = data.load_reconstruction(fn_resplit)
else:
reconstructions = data.load_reconstruction()
reconstructions = [reconstructions[rec_a], reconstructions[rec_b]]
coords0 = triangulate_gcps(gcps, reconstructions[0])
coords1 = triangulate_gcps(gcps, reconstructions[1])
n_valid_0 = sum(c is not None for c in coords0)
logger.debug(f"Triangulated {n_valid_0}/{len(gcps)} gcps for rec #{rec_a}")
n_valid_1 = sum(c is not None for c in coords1)
logger.debug(f"Triangulated {n_valid_1}/{len(gcps)} gcps for rec #{rec_b}")
try:
s, A, b = find_alignment(coords1, coords0)
apply_similarity(reconstructions[1], s, A, b)
except ValueError:
logger.warning(f"Could not rigidly align rec #{rec_b} to rec #{rec_a}")
return
logger.info(f"Rigidly aligned rec #{rec_b} to rec #{rec_a}")
else: # Image - to - reconstruction annotation
reconstructions = data.load_reconstruction()
base = reconstructions[rec_a]
resected = resect_annotated_single_images(base, gcps, camera_models, data)
reconstructions = [base, resected]
else:
logger.debug(
"Only one reconstruction in reconstruction.json. Will only to 3d-3d alignment if any"
)
align_external_3d_models_to_reconstruction(
data, gcps, reconstructions[0], rec_a
)
return
logger.debug(f"Aligning annotations, if any, to rec #{rec_a}")
align_external_3d_models_to_reconstruction(data, gcps, reconstructions[0], rec_a)
# Set the GPS constraint of the moved/resected shots to the manually-aligned position
for shot in reconstructions[1].shots.values():
shot.metadata.gps_position.value = shot.pose.get_origin()
data.save_reconstruction(reconstructions, fn_rigid)
logger.info("Merging reconstructions")
merged = merge_reconstructions(reconstructions, tracks_manager)
# data.save_reconstruction(
# [merged], f"reconstruction_merged_{rec_a}x{rec_b}.json"
# )
# Scale the GPS DOP with the number of shots to ensure GCPs are used to align
for shot in merged.shots.values():
shot.metadata.gps_accuracy.value = 0.5 * len(merged.shots)
gcp_alignment = {"after_rigid": gcp_geopositional_error(gcps, merged)}
logger.info(
"GCP errors after rigid alignment:\n"
+ "\n".join(
"[{}]: {:.2f} m / {:.2f} m (planar)".format(
k, v["error"], v["error_planar"]
)
for k, v in gcp_alignment["after_rigid"].items()
)
)
if not rigid:
data.config["bundle_max_iterations"] = 200
data.config["bundle_use_gcp"] = True
logger.info("Running BA on merged reconstructions")
# orec.align_reconstruction(merged, None, data.config)
orec.bundle(merged, camera_models, {}, gcp=gcps, config=data.config)
data.save_reconstruction(
[merged], f"reconstruction_gcp_ba_{rec_a}x{rec_b}.json"
)
gcp_alignment["after_bundle"] = gcp_geopositional_error(gcps, merged)
logger.info(
"GCP errors after bundle:\n"
+ "\n".join(
"[{}]: {:.2f} m / {:.2f} m (planar)".format(
k, v["error"], v["error_planar"]
)
for k, v in gcp_alignment["after_bundle"].items()
)
)
with open(f"{data.data_path}/gcp_alignment_{rec_a}x{rec_b}.json", "w") as f:
json.dump(gcp_alignment, f, indent=4, sort_keys=True)
# Reproject GCPs with a very loose threshold so that we get a point every time
# These reprojections are only used for feedback in any case
gcp_reprojections = reproject_gcps(gcps, merged, reproj_threshold=10)
reprojection_errors = get_sorted_reprojection_errors(gcp_reprojections)
err_values = [t[2] for t in reprojection_errors]
max_reprojection_error = np.max(err_values)
median_reprojection_error = np.median(err_values)
with open(f"{data.data_path}/gcp_reprojections_{rec_a}x{rec_b}.json", "w") as f:
json.dump(gcp_reprojections, f, indent=4, sort_keys=True)
n_bad_gcp_annotations = int(sum(t[2] > px_threshold for t in reprojection_errors))
if n_bad_gcp_annotations > 0:
logger.info(f"{n_bad_gcp_annotations} large reprojection errors:")
for t in reprojection_errors:
if t[2] > px_threshold:
logger.info(t)
gcp_std = compute_gcp_std(gcp_reprojections)
logger.info(f"GCP reprojection error STD: {gcp_std}")
resplit = resplit_reconstruction(merged, reconstructions)
data.save_reconstruction(resplit, fn_resplit)
if covariance:
# Re-triangulate to remove badly conditioned points
n_points = len(merged.points)
logger.info("Re-triangulating...")
backup = data.config["triangulation_min_ray_angle"]
data.config["triangulation_min_ray_angle"] = 2.0
orec.retriangulate(tracks_manager, merged, data.config)
orec.paint_reconstruction(data, tracks_manager, merged)
data.config["triangulation_min_ray_angle"] = backup
logger.info(
f"Re-triangulated. Removed {n_points - len(merged.points)}."
f" Kept {int(100*len(merged.points)/n_points)}%"
)
data.save_reconstruction(
[merged],
f"reconstruction_gcp_ba_retriangulated_{rec_a}x{rec_b}.json",
)
all_shots_std = []
# We run bundle by fixing one reconstruction.
# If we have two reconstructions, we do this twice, fixing each one.
_rec_ixs = [(0, 1), (1, 0)] if rec_b is not None else [(0, 1)]
for rec_ixs in _rec_ixs:
logger.info(f"Running BA with fixed images. Fixing rec #{rec_ixs[0]}")
fixed_images = set(reconstructions[rec_ixs[0]].shots.keys())
covariance_estimation_valid = bundle_with_fixed_images(
merged,
camera_models,
gcp=gcps,
gcp_std=gcp_std,
fixed_images=fixed_images,
config=data.config,
)
if not covariance_estimation_valid:
logger.info(
f"Could not get positional uncertainty for pair {rec_ixs} It could be because:"
"\na) there are not enough GCPs."
"\nb) they are badly distributed in 3D."
"\nc) there are some wrong annotations"
)
shots_std_this_pair = [
(shot, np.nan) for shot in reconstructions[rec_ixs[1]].shots
]
else:
shots_std_this_pair = []
for shot in merged.shots.values():
if shot.id in reconstructions[rec_ixs[1]].shots:
u, std_v = decompose_covariance(shot.covariance[3:, 3:])
std = np.linalg.norm(std_v)
shots_std_this_pair.append((shot.id, std))
logger.debug(f"{shot.id} std: {std}")
all_shots_std.extend(shots_std_this_pair)
std_values = [x[1] for x in all_shots_std]
n_nan_std = sum(np.isnan(std) for std in std_values)
n_good_std = sum(
std <= std_threshold for std in std_values if not np.isnan(std)
)
n_bad_std = len(std_values) - n_good_std - n_nan_std
# Average positional STD
median_shot_std = np.median(std_values)
# Save the shot STD to a file
with open(f"{data.data_path}/shots_std_{rec_a}x{rec_b}.csv", "w") as f:
s = sorted(all_shots_std, key=lambda t: -t[-1])
for t in s:
line = "{}, {}".format(*t)
f.write(line + "\n")
max_shot_std = s[0][1]
else:
n_nan_std = -1
n_bad_std = -1
n_good_std = -1
median_shot_std = -1
max_shot_std = -1
std_values = []
metrics = {
"n_reconstructions": len(data.load_reconstruction()),
"median_shot_std": median_shot_std,
"max_shot_std": max_shot_std,
"max_reprojection_error": max_reprojection_error,
"median_reprojection_error": median_reprojection_error,
"n_gcp": len(gcps),
"n_bad_gcp_annotations": n_bad_gcp_annotations,
"n_bad_position_std": int(n_bad_std),
"n_good_position_std": int(n_good_std),
"n_nan_position_std": int(n_nan_std),
"rec_a": rec_a,
"rec_b": rec_b,
}
logger.info(metrics)
p_metrics = f"{data.data_path}/run_ba_metrics_{rec_a}x{rec_b}.json"
with open(p_metrics, "w") as f:
json.dump(metrics, f, indent=4, sort_keys=True)
logger.info(f"Saved metrics to {p_metrics}")
logger.info("========================================")
logger.info("=============== Summary ================")
logger.info("========================================")
if n_bad_std == 0 and n_bad_gcp_annotations == 0:
logger.info(
f"No issues. All gcp reprojections are under {px_threshold}"
f" and all frames are localized within {std_threshold}m"
)
if n_bad_std != 0 or n_bad_gcp_annotations != 0:
if rigid:
logger.info("Positional uncertainty was not calculated. (--rigid was set).")
elif not covariance:
logger.info(
"Positional uncertainty was not calculated (--covariance not set)."
)
else:
logger.info(
f"{n_nan_std}/{len(std_values)} images with unknown error."
f"\n{n_good_std}/{len(std_values)} well-localized images."
f"\n{n_bad_std}/{len(std_values)} badly localized images."
)
if n_bad_gcp_annotations > 0:
logger.info(
f"{n_bad_gcp_annotations} annotations with large reprojection error."
" Worst offenders:"
)
stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp(
gcp_reprojections, px_threshold
)
gcps_sorted = sorted(
stats_bad_reprojections, key=lambda k: -stats_bad_reprojections[k]
)
for ix, gcp_id in enumerate(gcps_sorted[:5]):
n = stats_bad_reprojections[gcp_id]
if n > 0:
logger.info(f"#{ix+1} - {gcp_id}: {n} bad annotations")
else:
logger.info("No annotations with large reprojection errors")
