Build brisk map (nasa#488)

Added script to create a brisk map from an existing surf map.

localization/sparse_mapping/build_map_from_multiple_bags.md

@@ -22,32 +22,38 @@ If a bag contains multiple movements that contain sufficient overlap, these can
 
 After splicing, the original bags can be removed or moved elsewhere.
 
-### 3. Build individual maps for each spliced bag
+### 3. Build individual SURF maps for each spliced bag
 Run    
-`rosrun sparse_mapping make_surf_maps.py`
+`rosrun sparse_mapping make_surf_maps.py -r robot_name -w world_name`
 
 This builds individual maps in parallel for each spliced bag in the directory. The mapping process first removes low movement images from a bagfile to prevent triangulation issues and make the mapping process more efficient, then builds a surf map (detects image features, matches features, runs incremental bundle adjustment using successive images, runs a final round of bundle adjustment on the whole map).
 
 
-### 4. Merge maps
+### 4. Merge SURF maps
 A common merge strategy for a set of movements is to identify the longest movement that covers the most of an area and incrementally merge in maps that have sufficient overlap with this. Use:    
 `rosrun sparse_mapping merge_maps larger_map_name smaller_map_name -output_map combined_map_name -num_image_overlaps_at_endpoints 1000000`
 
-Sometimes an existing map is available that may contain areas not covered in the new bags. In this case, first merge the new movements together using the above strategy, then merge the resultant combined new movements map into the existing map. 
+First merge the new movements together using the above strategy, then optionally merge the resultant combined new movements map into an already existing map (that has some overlap with the new combined movements maps). 
 
-### 5. Register map against real world
-Since mapping is perfomed using monocular images, no set scale is provied and the map needs to be registered using defined real world points before using for localization. 
+### 5. Register SURF map using provided world points
+Since mapping is perfomed using monocular images, no set scale is provied and the map needs to be registered using real world points before being used for localization. Additionally, providing known point locations for images in the map can improve the accuracy of the map creation. 
 
 First copy the map that should be registered then run:    
 `rosrun sparse_mapping build_map -registration registration_points.pto registration_points.txt -output_map registered_map_name` 
 
 The creation of registration points is detailed in `build_map.md`, images from the map are used to manually select feature locations and 3D points for these features are selected using a 3D model of the mapped environment provided externally.
 
-### 6. Verify the resulting map
+### 6. Build BRISK map for localization 
 Use:     
-`rosrun sparse_mapping run_graph_bag_and_plot_results bag_name map_name config_path --generate-image-features -r robot_config -w world_name`  
-to test localization with the map. 
+`rosrun sparse_mapping make_brisk_map.py surf_map_name -r robot_name -w world_name -d map_directory`  
+to rebuild the SURF map with BRISK features for use with localization. 
 
-The bags used here should not have been used for map creation but should contain data in the area of the map. They additionally need IMU data, if this is not available image registration can be testing using the sparse_mapping_pose_adder tool. Make sure to include the `--generate-image-features` option since image features in the bag are recorded using matches with whatever map was used when the bag was recorded. 
 
-This script creates a pdf plotting localization accuracy and map-based pose estimates. If the map is well made, localization pose estimates should be relatively smooth and consistent. Jumps in poses tend to indicate the portions of the map may not be well aligned.
+### 7. Verify BRISK map using localization
+Use:     
+`rosrun sparse_mapping run_graph_bag_and_plot_results bag_name brisk_map_name config_path --generate-image-features -r robot_config -w world_name`  
+to test the map using localization. 
+
+The bags used here should not have been used for map creation but should contain data in the area of the map. If they contain IMU data full localization results will be generated, otherwise only pose estimates using the bag images registered with the provided map will be plotted. Make sure to include the `--generate-image-features` option to generate new map matches with the provided map. 
+
+This script creates a pdf plotting localization accuracy and map-based pose estimates. If the map is well made, the localization and pose estimates should be relatively smooth and consistent. Jumps in these tend to indicate that portions of the map may not be well aligned.

localization/sparse_mapping/scripts/make_brisk_map.py

@@ -0,0 +1,101 @@
+#!/usr/bin/python
+#
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+"""
+Generates a new brisk map with a vocab database using the provided surf map.
+"""
+
+import argparse
+import os
+import shutil
+import sys
+
+import localization_common.utilities as lu
+
+
+def make_brisk_map(surf_map, world, robot_name, map_directory=None):
+    # 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, robot_config_file)
+    os.environ["ASTROBEE_WORLD"] = world
+
+    # Convert SURF to BRISK map
+    # Get full path to output file to avoid permission errors when running
+    # command in maps directory
+    map_name = lu.basename(surf_map)
+    build_brisk_map_output_file = os.path.join(
+        os.getcwd(), "rebuild_map_as_brisk_map.txt"
+    )
+    brisk_map = map_name + ".brisk.map"
+    shutil.copyfile(surf_map, brisk_map)
+    brisk_map_full_path = os.path.abspath(brisk_map)
+    path = os.getcwd()
+    # Change to map directory so relative image locations are correct if necessary
+    if map_directory:
+        os.chdir(map_directory)
+    build_brisk_map_command = (
+        "rosrun sparse_mapping build_map -rebuild -histogram_equalization -output_map "
+        + brisk_map_full_path
+    )
+    lu.run_command_and_save_output(build_brisk_map_command, build_brisk_map_output_file)
+    # Change back to original directory so final map is saved there
+    if map_directory:
+        os.chdir(path)
+
+    # Create vocabdb
+    brisk_vocabdb_map = map_name + ".brisk.vocabdb.map"
+    shutil.copyfile(brisk_map, brisk_vocabdb_map)
+    add_vocabdb_command = (
+        "rosrun sparse_mapping build_map -vocab_db -output_map " + brisk_vocabdb_map
+    )
+    lu.run_command_and_save_output(add_vocabdb_command, "build_vocabdb.txt")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter
+    )
+    parser.add_argument(
+        "surf_map", help="Input SURF map to generate the BRISK map for."
+    )
+    parser.add_argument("-w", "--world", default="iss")
+    parser.add_argument("-r", "--robot-name", default="bumble")
+    parser.add_argument(
+        "-d",
+        "--map-directory",
+        default=None,
+        help="Location where surf map was created, needed to load images for BRISK map building since relative paths are used during map creation for the image locations. Defaults to current working directory.",
+    )
+
+    args = parser.parse_args()
+    if not os.path.isfile(args.surf_map):
+        print("SURF map " + args.surf_map + " does not exist.")
+        sys.exit()
+
+    if args.map_directory and not os.path.isdir(args.map_directory):
+        print("Map directory " + args.map_directory + " does not exist.")
+        sys.exit()
+
+    surf_map = os.path.abspath(args.surf_map)
+    if args.map_directory:
+        args.map_directory = os.path.abspath(args.map_directory)
+    make_brisk_map(surf_map, args.world, args.robot_name, args.map_directory)

localization/sparse_mapping/scripts/make_surf_map.py

@@ -60,9 +60,7 @@ def make_surf_map(
     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_ROBOT"] = os.path.join(astrobee_path, robot_config_file)
     os.environ["ASTROBEE_WORLD"] = world
 
     # Build map