From 2fbf7894dfec5991db2798a2eda69de6c5807e3e Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 16:11:11 -0800 Subject: [PATCH] theia_map: Allow importing an undistorted map --- localization/sparse_mapping/theia_map.md | 5 ++- .../sparse_mapping/tools/build_theia_map.py | 45 ++++++++++++++----- 2 files changed, 39 insertions(+), 11 deletions(-) diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 065e81c93a..0817d4df25 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -68,7 +68,7 @@ directory needs to be added to the PATH. # Run the Astrobee wrapper around the Theia tools The conda environment set up earlier will confuse the lookup the -depencencies for the Astrobee libraries. Hence the lines ``conda`` added +dependencies for the Astrobee libraries. Hence the lines ``conda`` added to one's ``.bashrc`` should be removed, the bash shell restarted, and one should ensure that the ``env`` command has no mentions of conda. @@ -101,6 +101,9 @@ This tool has the following command-line options: Theia. --work_dir: A temporary work directory to be deleted by the user later. + --keep_undistorted_images: Do not replace the undistorted images + Theia used with the original distorted ones in the sparse map + imported from Theia. This is for testing purposes. --help: Show this help message and exit. # Auxiliary import_map tool diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index 6b73e29dff..388e33cc45 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -103,12 +103,17 @@ def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): if which("export_to_nvm_file") is None: raise Exception("Cannot find the 'export_to_nvm_file' program in PATH.") - + if args.keep_undistorted_images and (not args.skip_rebuilding): + raise Exception("Cannot rebuild the map if it has undistorted images.") + def process_args(args): """ Set up the parser and parse the args. """ + # Number of arguments before starting to parse them + num_input_args = len(sys.argv) + # Extract some paths before the args are parsed src_path = os.path.dirname(args[0]) exec_path = os.path.dirname( @@ -118,7 +123,7 @@ def process_args(args): import_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/import_map") build_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/build_map") - parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") + parser = argparse.ArgumentParser(description="") parser.add_argument( "--theia_flags", dest="theia_flags", @@ -149,8 +154,21 @@ def process_args(args): help="A temporary work directory to be deleted by the user later.", ) + parser.add_argument( + "--keep_undistorted_images", + dest="keep_undistorted_images", + action="store_true", + help="Do not replace the undistorted images Theia used with the original " + + "distorted ones in the sparse map imported from Theia. This is for testing " + + "purposes.", + ) args = parser.parse_args() + # Print the help message if called with no arguments + if num_input_args <= 1: + parser.print_help() + sys.exit(1) + if args.theia_flags == "": args.theia_flags = os.path.dirname(src_path) + "/theia_flags.txt" @@ -207,7 +225,7 @@ def gen_undist_image_list(work_dir, dist_image_list): def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): calib_file = work_dir + "/" + "theia_calibration.json" - + intrinsics_str = "" # Parse the intrinsics with open(undist_intrinsics_file, "r") as fh: for line in fh: @@ -216,6 +234,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): if line[0] == "#": continue + intrinsics_str = line.rstrip() # will need this later intrinsics = line.split() if len(intrinsics) < 5: raise Exception( @@ -256,8 +275,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): fh.write("]\n") fh.write("}\n") - return calib_file - + return calib_file, intrinsics_str if __name__ == "__main__": @@ -289,7 +307,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): ] run_cmd(cmd) - calib_file = gen_theia_calib_file( + (calib_file, intrinsics_str) = gen_theia_calib_file( args.work_dir, undist_images, undist_intrinsics_file ) recon_file = args.work_dir + "/run" @@ -341,10 +359,17 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): "-output_map", args.output_map, "-undistorted_images_list", - undist_image_list, - "-distorted_images_list", - args.image_list, - ] + undist_image_list] + + if not args.keep_undistorted_images: + cmd += ["-distorted_images_list", + args.image_list, + ] + else: + cmd += ["-undistorted_camera_params", + intrinsics_str, + ] + run_cmd(cmd) if not args.skip_rebuilding: