def run_groundtruth(params):
output_directory = utilities.basename(params.bagfile) + "_groundtruth"
groundtruth_command = (
"rosrun localization_analysis make_groundtruth.py "
+ params.bagfile
+ " "
+ params.base_surf_map
+ " "
+ params.maps_directory
+ " "
+ params.loc_map
+ " "
+ params.config_path
+ " -o "
+ output_directory
+ " -w "
+ params.world
+ " -i "
+ params.image_topic
+ " -r "
+ params.robot_name
)
if not bool(params.use_image_features):
groundtruth_command += " --generate-image-features"
output_file = utilities.basename(params.bagfile) + "_groundtruth.txt"
utilities.run_command_and_save_output(groundtruth_command, output_file)
def create_groundtruth(
bagfile, base_surf_map, maps_directory, map_name, world, robot_name
):
gt_images_dir = "gt_images_" + utilities.basename(bagfile)
os.mkdir(gt_images_dir)
gt_images = os.path.abspath(gt_images_dir)
extract_images_command = (
"rosrun localization_node extract_image_bag "
+ bagfile
+ " -use_timestamp_as_image_name -image_topic /mgt/img_sampler/nav_cam/image_record -output_directory "
+ gt_images
)
utilities.run_command_and_save_output(extract_images_command, "extract_images.txt")
all_gt_images = os.path.join(gt_images, "*.jpg")
select_images_command = (
"rosrun sparse_mapping select_images -density_factor 1.4 " + all_gt_images
)
utilities.run_command_and_save_output(select_images_command, "select_images.txt")
# Set environment variables
home = os.path.expanduser("~")
robot_config_file = os.path.join("config/robots", robot_name + ".config")
astrobee_path = os.path.join(home, "astrobee/src/astrobee")
os.environ["ASTROBEE_RESOURCE_DIR"] = os.path.join(astrobee_path, "resources")
os.environ["ASTROBEE_CONFIG_DIR"] = os.path.join(astrobee_path, "config")
os.environ["ASTROBEE_ROBOT"] = os.path.join(
astrobee_path, "config/robots/bumble.config"
)
os.environ["ASTROBEE_WORLD"] = world
# Build groundtruth
groundtruth_map = map_name + ".map"
build_map_command = (
"rosrun sparse_mapping build_map "
+ all_gt_images
+ " -output_map "
+ groundtruth_map
+ " -feature_detection -feature_matching -track_building -incremental_ba -bundle_adjustment -histogram_equalization -num_subsequent_images 100"
)
utilities.run_command_and_save_output(build_map_command, "build_map.txt")
# Merge with base map
groundtruth_surf_map = map_name + ".surf.map"
merge_map_command = (
"rosrun sparse_mapping merge_maps "
+ base_surf_map
+ " "
+ groundtruth_map
+ " -output_map "
+ groundtruth_surf_map
+ " -num_image_overlaps_at_endpoints 100000000 -skip_bundle_adjustment"
)
utilities.run_command_and_save_output(merge_map_command, "merge_map.txt")
# Link maps directory since conversion to BRISK map needs
# image files to appear to be in correct relative path
os.symlink(maps_directory, "maps")
maps_gt_images = os.path.join("maps", gt_images_dir)
os.symlink(gt_images, maps_gt_images)
# Convert SURF to BRISK map
# Get full path to output file to avoid permission errors when running
# command in maps directory
rebuild_output_file = os.path.join(os.getcwd(), "rebuild_map_as_brisk_map.txt")
groundtruth_brisk_map = map_name + ".brisk.map"
shutil.copyfile(groundtruth_surf_map, groundtruth_brisk_map)
groundtruth_brisk_map_full_path = os.path.abspath(groundtruth_brisk_map)
gt_path = os.getcwd()
os.chdir("maps")
rebuild_map_command = (
"rosrun sparse_mapping build_map -rebuild -histogram_equalization -output_map "
+ groundtruth_brisk_map_full_path
)
utilities.run_command_and_save_output(rebuild_map_command, rebuild_output_file)
# Use gt_path since relative commands would now be wrt maps directory simlink
os.chdir(gt_path)
# Create vocabdb
groundtruth_brisk_vocabdb_map = map_name + ".brisk.vocabdb.map"
shutil.copyfile(groundtruth_brisk_map, groundtruth_brisk_vocabdb_map)
add_vocabdb_command = (
"rosrun sparse_mapping build_map -vocab_db -output_map "
+ groundtruth_brisk_vocabdb_map
)
utilities.run_command_and_save_output(add_vocabdb_command, "build_vocabdb.txt")
# Remove simlinks
os.unlink(maps_gt_images)
os.unlink("maps")
make_map.make_map(bagfile, map_name, args.world, args.robot_name,
base_surf_map, maps_directory)
robot_config = "config/robots/" + args.robot_name + ".config"
groundtruth_bag = map_name + ".bag"
groundtruth_map_file = map_name + ".brisk.vocabdb.map"
groundtruth_pdf = "groundtruth.pdf"
groundtruth_csv = "groundtruth.csv"
make_groundtruth_command = (
"rosrun localization_analysis run_graph_bag_and_plot_results.py " +
bagfile + " " + groundtruth_map_file + " " + args.config_path +
" -i " + args.image_topic + " -r " + robot_config + " -w " +
args.world + " -o " + groundtruth_bag + " --output-file " +
groundtruth_pdf + " --output-csv-file " + groundtruth_csv +
" --generate-image-features")
utilities.run_command_and_save_output(make_groundtruth_command,
"make_groundtruth.txt")
os.rename("run_graph_bag_command.txt",
"groundtruth_run_graph_bag_command.txt")
loc_results_bag = bag_prefix + "_results.bag"
loc_pdf = "loc_results.pdf"
loc_csv = "loc_results.csv"
get_loc_results_command = (
"rosrun localization_analysis run_graph_bag_and_plot_results.py " +
bagfile + " " + args.loc_map + " " + args.config_path + " -i " +
args.image_topic + " -r " + robot_config + " -w " + args.world +
" -o " + loc_results_bag + " --output-file " + loc_pdf +
" --output-csv-file " + loc_csv + " -g " + groundtruth_bag)
if not args.use_image_features:
get_loc_results_command += " --generate-image-features"
utilities.run_command_and_save_output(get_loc_results_command,
+ bagfile
+ " "
+ map_file
+ " "
+ args.config_path
+ " -r "
+ args.robot_config
+ " -w "
+ args.world
+ " --use-image-features "
+ (str(args.use_image_features)).lower()
+ " -o "
+ args.output_bagfile
)
utilities.run_command_and_save_output(
run_graph_bag_command, "run_graph_bag_command.txt"
)
# Plot results
plot_results_command = (
"rosrun localization_analysis plot_results.py "
+ args.output_bagfile
+ " --output-file "
+ args.output_file
+ " --output-csv-file "
+ args.output_csv_file
)
if args.groundtruth_bagfile:
plot_results_command += " -g " + args.groundtruth_bagfile
utilities.run_command_and_save_output(
plot_results_command, "plot_results_command.txt"