diff --git a/tools/localization_analysis/scripts/make_groundtruth.py b/tools/localization_analysis/scripts/make_groundtruth.py index 2bce0fafa0..9d1fe226d2 100755 --- a/tools/localization_analysis/scripts/make_groundtruth.py +++ b/tools/localization_analysis/scripts/make_groundtruth.py @@ -65,6 +65,13 @@ default=None, help="Prefix for generated map names. Defaults to bagfile name.", ) + parser.add_argument( + "-l", + "--max-low-movement-mean-distance", + type=float, + default=0.09, + help="Threshold for sequential image removal, the higher the more images removed.", + ) parser.add_argument( "--generate-image-features", dest="use_image_features", @@ -113,6 +120,7 @@ args.world, args.robot_name, args.histogram_equalization, + args.max_low_movement_mean_distance, base_surf_map, maps_directory, ) diff --git a/tools/localization_analysis/scripts/make_map.py b/tools/localization_analysis/scripts/make_map.py index f05b27dcea..4948b6a444 100755 --- a/tools/localization_analysis/scripts/make_map.py +++ b/tools/localization_analysis/scripts/make_map.py @@ -35,6 +35,7 @@ def make_map( world, robot_name, histogram_equalization, + max_low_movement_mean_distance, base_surf_map=None, maps_directory=None, ): @@ -52,7 +53,10 @@ def make_map( lu.run_command_and_save_output(extract_images_command, "extract_images.txt") remove_low_movement_images_command = ( - "rosrun sparse_mapping remove_low_movement_images " + bag_images + "rosrun sparse_mapping remove_low_movement_images " + + bag_images + + " -m " + + str(max_low_movement_mean_distance) ) lu.run_command_and_save_output( remove_low_movement_images_command, basename + "_remove_low_movement_images.txt" @@ -83,6 +87,7 @@ def make_map( build_map_command += " -histogram_equalization" lu.run_command_and_save_output(build_map_command, "build_map.txt") + linked_map_images = False if merge_with_base_map: merged_surf_map = map_name + ".surf.map" merge_map_command = ( @@ -101,7 +106,9 @@ def make_map( # image files to appear to be in correct relative path os.symlink(maps_directory, "maps") maps_bag_images = os.path.join("maps", bag_images_dir) - os.symlink(bag_images, maps_bag_images) + if not os.path.isdir(maps_bag_images): + os.symlink(bag_images, maps_bag_images) + linked_map_images = True # Convert SURF to BRISK map # Get full path to output file to avoid permission errors when running @@ -134,7 +141,8 @@ def make_map( if merge_with_base_map: # Remove simlinks - os.unlink(maps_bag_images) + if linked_map_images: + os.unlink(maps_bag_images) os.unlink("maps") @@ -159,6 +167,13 @@ def make_map( parser.add_argument("-w", "--world", default="iss") parser.add_argument("-r", "--robot-name", default="bumble") parser.add_argument("-m", "--map-name", default="bag_map") + parser.add_argument( + "-l", + "--max-low-movement-mean-distance", + type=float, + default=0.15, + help="Threshold for sequential image removal, the higher the more images removed.", + ) parser.add_argument( "-n", "--no-histogram_equalization", @@ -205,6 +220,7 @@ def make_map( args.world, args.robot_name, args.histogram_equalization, + args.max_low_movement_mean_distance, base_surf_map, maps_directory, )