Tutorial: Mapping and Localization with Isaac Perceptor ================================================================================================== This tutorial describes how to use the offline mapping capabilities of Isaac Perceptor including visual global (cuVGL) localization map creation, cuVSLAM map creation, and occupancy grid map creation, and how to run Isaac Perceptor with localization in the map. Map Types --------- Isaac Perceptor is able to produce a few different types of maps when provided with rosbags. The following list is the set of maps that are generated by the :code:`create_map_offline.py` script shown below. **cuVSLAM Map** cuVSLAM is able to create a map and load a saved map of an environment and use that map to improve localization performance and maintain a consistent global frame of reference. **Visual Global Localization Map** During startup or after losing localization, visual global localization can be used to globally localize on a previously created map and operate in a shared global frame of reference relative. For more info about visual global localization, see: :doc:`Concepts: Visual Global Localization ` **Occupancy Grid Map** Navigation packages such as `nav2 `_ use an occupancy grid map in order represent the obstacles in a map. Isaac Perceptor uses :doc:`cuVSLAM ` and :doc:`Nvblox ` to build an occupancy grid map using cameras. .. note:: The generated occupancy map is only intended to be used for global path planning. Specifically it is not intended to be used for 2d lidar localization. The generated map may contain slight artifacts and imprecisions and therefore is expected to be used with a planner that inflates obstacles. .. note:: Because the ROS2 rosbag replay is nondeterministic, each run of the occupancy map creation may produce slightly different map. Data Collection --------------- 1. Complete the instruction in the following doc to run the Nova recorder to record data :ref:`isaac_ros_nova_recorder Quickstart`. 2. Use :ir_repo:`nova-carter_hawk-4_no-3d-lidar ` configuration for data recording with the `Nova Carter`, which records the front, back, left and right stereo cameras. Use :ir_repo:`hawk-3.yaml ` configuration for data recording with the `Nova Orin Deverloper Kit`, which records the front, left, and right stereo cameras. 3. Drive the robot around the environment and capture as much of the environment as possible. A vocabulary built with complete features of the environment and environments with varied features are needed for image retrieval. Featureless environments or those with predominantly repetitive structures can result in failed localization. We recommend moving the robot for at least 1 minute within a 5x5m area where it can observe various objects like chairs, tables, shelves, and other office furniture. 4. To generate a more accurate map, follow the guidelines for the robot's trajectories during data collection: a. **Closed Loop Segments:** Build mapping trajectories in loops or segments of around 5-10 meters. This ensures the same area is captured from different angles, which improves map detail and reduces drift. b. **Avoid Long Straight Paths:** Long drives in unexplored areas accumulate drift risk. Use short, closed loops instead. c. **Vary Camera Angles:** Capturing locations from multiple angles makes the visual map more reliable for future localization. Schematic examples of efficient mapping paths in both narrow corridors and open spaces, demonstrating optimal patterns for capturing comprehensive visual data and reducing tracking drift. .. note:: To mitigate the effects of tracking drift, :doc:`Multi-camera SLAM ` is used. For optimal pose correction and map accuracy, Loop Closing must be effectively exploited. This mechanism corrects the robot's pose when the robot revisits a previously mapped location. .. figure:: :ir_lfs:`` :width: 800px :alt: Visualization of the mapping trajectory in corridor :align: center .. figure:: :ir_lfs:`` :width: 800px :alt: Visualization of the mapping trajectory in open space :align: center 5. For localization rosbag collection, make sure to record at least 10s stationary data at the beginning of the rosbag to allow the global localization to initialize correctly. The localization trajectory should be in the mapping area and not deviate too much from the mapping trajectory. Running the Application ----------------------- First, SSH into your robot (instructions for the :ref:`Nova Carter` and the :ref:`Nova Orin Developer Kit`). Once you have SSHd into the robot, follow the instructions below to map and localize against maps. Creating Maps ------------- .. warning:: Map Creation will use all the CPU and GPU resources, make sure you do not have anything important running at the same time. .. warning:: Glass and very thin structures such as wires and cable railings cannot be detected by the current stereo networks used for depth estimation. If you have such structures in your environment, you might need to modify the environment so that there are larger visible features so that unnavigable areas are correctly estimated. .. note:: The quality of the occupancy map is dependent on the the disparity data provided to nvblox. Degraded results may occur in wall corners. Improved results may be achieved with alternative stereo modalities, such as a different stereo camera or other AI/DNNs for stereo disparity estimation. Make sure the rosbag and output map folder path are mounted to the Docker container. You can add them in ``${ISAAC_ROS_WS}/src/isaac_ros_common/scripts/.isaac_ros_dev-dockerargs`` then restart the container. Install `isaac_mapping_ros` on your `Nova Carter` or `Nova Orin Developer Kit` and create the maps mentioned above using the following commands: .. tabs:: .. tab:: Binary Package .. run_ros_app_binary:: :repo_setup: :packages_up_to: isaac_mapping_ros :package: isaac_mapping_ros :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :extra_packages: :launch_args: :app: create_map_offline.py --sensor_data_bag= --base_output_folder= :command: run You can monitor the progress of steps, including image extraction, running visual SLAM, depth inference, occupancy map generation and global localization map creation. Upon successful completion, you should see a message confirming that maps have been created. A good occupancy map features well-aligned objects such as walls and overlapping occupancy when the robot revisits the same locations. Misalignment in the map is typically caused by pose errors. To get a better map, try collecting a rosbag with more small loops in an environment rich with visual features. Ensure one map creation job runs at a time within a :code:`ROS_DOMAIN_ID`. You can run simultaneous jobs within different :code:`ROS_DOMAIN_ID`. If depth inference step fails or hangs, it's usually because the previous ESS run didn't exit cleanly. You can manually kill the ROS process or try to restart the Docker container and run the command again. Running Perceptor with Localization -------------------------------------- .. note:: When localizing against a created map, distance from the trajectory taken during mapping and the amount of change in the environment will heavily effect the time required and the chance of localizing. It is typically advised to stay within a meter of the trajectory of the initial map. If you are having difficulties localizing against a map and the environment has changed significantly, try remapping the environment. .. note:: When enabling SLAM mode, you might see warnings of message drops from time to time. This is due to the extra compute required for localization and should not cause any issues. However, if you start experiencing degraded performance, make sure there is enough compute available for your configuration. For example, if any other processes are running in the background which are using CPU, GPU, or memory bandwidth you might experience reduced localization performance. After you have created the maps in the tutorial above, you can use the maps for running Perceptor with localization. First set the two environment variables for the location of the recordings and the map output folder. Modify those two variables based on your setup, for example, on Nova Carter: .. code:: export RECORDINGS_FOLDER=/mnt/nova_ssd/recordings export MAPS_FOLDER=/mnt/nova_ssd/maps If you have a `Nova Carter`, install or build `nova_carter_bringup` and launch the app using the following command: .. run_ros_app:: :repo_setup: :repo: nova_carter :image: isaac/nova_carter_bringup:release_3.2-aarch64 :packages_up_to: nova_carter_bringup :package: nova_carter_bringup :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :additional_docker_options: -v $RECORDINGS_FOLDER:$RECORDINGS_FOLDER -v $MAPS_FOLDER:$MAPS_FOLDER :app: perceptor.launch.py stereo_camera_configuration:=front_left_right_configuration disable_vgl:=False vslam_load_map_folder_path:=/cuvslam_map/ vgl_map_dir:=/cuvgl_map/ occupancy_map_yaml_file:=/occupancy_map.yaml vslam_enable_slam:=True :command: launch The option :code:`--disable_vgl` determines if visual global localization is turned off. The options :code:`--vslam_load_map_folder_path` and :code:`--vgl_map_dir` refer to folders created in the above tutorial. :code:`` refers to the option :code:`--output_folder` specified in the map creation tutorial above. If :code:`--occupancy_map_yaml_file` is specified, Nav2's `Map Server `_ is used to publish the occupancy grid as a :code:`nav2::OccupancyGridMap` message. If you have a `Nova Orin Developer Kit`, install or build `nova_developer_kit_bringup` and launch the app using the following command: .. run_ros_app:: :repo_setup: :repo: nova_developer_kit :image: isaac/nova_developer_kit_bringup:release_3.2-aarch64 :packages_up_to: nova_developer_kit_bringup :package: nova_developer_kit_bringup :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :additional_docker_options: -v $RECORDINGS_FOLDER:$RECORDINGS_FOLDER -v $MAPS_FOLDER:$MAPS_FOLDER :app: perceptor.launch.py stereo_camera_configuration:=front_left_right_configuration disable_vgl:=False vslam_load_map_folder_path:=/cuvslam_map/ vgl_map_dir:=/cuvgl_map/ occupancy_map_yaml_file:=/occupancy_map.yaml vslam_enable_slam:=True :command: launch To run localization with a prerecorded rosbag, use :code:`mode:=rosbag rosbag:=` to specify the rosbag path. The command to run it on a `Nova Carter` is as follows: .. run_ros_app:: :repo_setup: :repo: nova_carter :image: isaac/nova_carter_bringup:release_3.2-aarch64 :packages_up_to: nova_carter_bringup :package: nova_carter_bringup :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :additional_docker_options: -v $RECORDINGS_FOLDER:$RECORDINGS_FOLDER -v $MAPS_FOLDER:$MAPS_FOLDER :app: perceptor.launch.py stereo_camera_configuration:=front_left_right_configuration disable_vgl:=False vslam_load_map_folder_path:=/cuvslam_map/ vgl_map_dir:=/cuvgl_map/ occupancy_map_yaml_file:=/occupancy_map.yaml vslam_enable_slam:=True mode:=rosbag rosbag:= :command: launch If the `odom` frame already exists in the `/tf` message in the rosbag, it may conflict with the `odom` frame generated by cuVSLAM. To resolve it, use :code:`replay_additional_args:="--remap /tf:=/tf_old"` to remap the existing `/tf` topic. Replay is at real time by default. You can adjust the replay rate by :code:`replay_rate:=`. When running a short localization bag, use :code:`replay_rate:=0.1` to slow down the replay, allowing more localization triggers and increasing the chances of successful localization. Visualization Setup ------------------------- Follow the steps to set up visualization, then you should see a visualization similar to the one in the quickstart example below: 1. Follow the :ref:`Foxglove Setup` tutorial to launch Foxglove. 2. Download :ir_repo:`nova_carter_perceptor_localization.json ` layout configuration file. 3. Import the downloaded layout file into Foxglove. Quickstart with a Pre-recorded Rosbag ---------------------------------------- 1. Download ``r2b_galileo`` dataset from the `r2b 2024 dataset on NGC `__. It is a 20-second bag containing synchronized images from four stereo camera pairs. Place the dataset at ``$ISAAC_ROS_WS/isaac_ros_assets/r2b_2024/r2b_galileo``. 2. To create the map from the downloaded ``r2b_galileo`` rosbag, run the following command: .. tabs:: .. tab:: Binary Package .. run_ros_app_binary:: :repo_setup: :packages_up_to: isaac_mapping_ros :package: isaac_mapping_ros :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :app: create_map_offline.py --sensor_data_bag=$ISAAC_ROS_WS/isaac_ros_assets/r2b_2024/r2b_galileo --base_output_folder=$ISAAC_ROS_WS/isaac_ros_assets/maps :command: run 3. To run visual global localization, visual SLAM and the camera-based perception with the map generated in the previous step, run the following command: .. note:: We use replay rate of 0.1 for the example because the rosbag is short and does not contain stationary frames at the beginning. Slowing down rosbag replay helps global localization to initialize correctly. .. run_ros_app:: :repo_setup: :repo: nova_carter :image: isaac/nova_carter_bringup:release_3.2-aarch64 :packages_up_to: nova_carter_bringup :package: nova_carter_bringup :asset_packages: isaac_ros_ess_models_install:isaac_ros_peoplesemseg_models_install:isaac_ros_peoplesemseg_models_install :asset_scripts: install_ess_models.sh:install_peoplesemsegnet_vanilla.sh:install_peoplesemsegnet_shuffleseg.sh :additional_docker_options: -v $ISAAC_ROS_WS:$ISAAC_ROS_WS :app: perceptor.launch.py rosbag:=$ISAAC_ROS_WS/isaac_ros_assets/r2b_2024/r2b_galileo mode:=rosbag stereo_camera_configuration:=front_left_right_configuration disable_vgl:=False vslam_load_map_folder_path:=/cuvslam_map vgl_map_dir:=/cuvgl_map occupancy_map_yaml_file:=/occupancy_map.yaml replay_rate:=0.1 vslam_enable_slam:=True :command: launch The ``map_dir`` specifies the path to the output map directory generated by the map creation step. 4. Visualize the outputs: .. figure:: :ir_lfs:`` :align: center :width: 800px You will see the `map` and `odom` frames nearly overlapping at the initial position on the left, and the `baselink` frame moving to the right. Near the initial position, a green arrow represents the pose hint sent from visual global localization. The purple trajectory, following the `baselink` frame indicates the robot's motion path, computed by visual SLAM. In the diagnostic window, the `localized_in_exist_map` field indicates whether the robot is localized within the map. If this field is `false`, the robot pose and motion path is in the `odom` frame starting from the identity pose at the beginning of the motion. Once the field changes to `true`, the robot is successfully localized to the `map` frame, and its motion path aligns with the map. After you have generated the maps using the above script, refer to the following tutorials to visualize additional perception outputs: * :doc:`Tutorial: Camera-based 3D Perception with Isaac Perceptor on the Nova Carter ` * :doc:`Tutorial: Camera-based 3D Perception with Isaac Perceptor on the Nova Orin Developer Kit `