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")