You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.01.
2D Occupancy Mapping
This mapping feature uses the depth sensor to obtain a 2.5D height map of the ground plane area, which can then be converted into a 2D occupancy grid map used in robot navigation.

Fig. 63 Occupancy Mapping Flowchart
Requirements
To use the mapping feature to its best capabilities, you require:
An installation of SLAMcore’s “SLAMcore Tools” package v21.03 or above
An installation of SLAMcore’s ROS1 Melodic package v21.03 or above (see SLAMcore ROS1 Wrapper) or SLAMcore’s ROS2 Foxy package v21.05 or above (see SLAMcore ROS2 Wrapper)
An environment with a single ground plane (multi-level floors or slopes are not supported).
Usage
Step 1 - Record a dataset of the entire test environment
With depth stream enabled, record your entire test environment and cover every area thoroughly as this will be used to generate the occupancy grid map. For optimal results:
Traverse paths twice in both directions (e.g. go up and down a corridor twice),
Ensure loop closures,
Use a ground robot on a single-level flat ground plane,
Keep the sensor angled downwards to point slightly towards the ground so that it “sees” the ground plane.
Note
Loop closure is supported in the mapping mode, however, even loop closure cannot always compensate for all the drift. Significant drift during mapping can result in very poor height / occupancy maps.
To record a dataset with depth enabled:
################## if using SLAMcore tools with GUI ###################
$ slamcore_dataset_recorder --depth
################# if using SLAMcore tools without GUI #################
$ slamcore_dataset_recorder_cli --depth -o <output-dir>
############################# if using ROS1 ############################
$ source /opt/ros/melodic/setup.bash
$ roslaunch slamcore_slam run_dataset_recorder.launch \
> override_realsense_depth:=true \
> realsense_depth_override_value:=true \
> output_dir:=<output-dir>
# To enable ROS visualisation, run in another terminal or machine (optional)
$ roslaunch slamcore_viz setup_monitoring.launch
############################# if using ROS2 ############################
$ source /opt/ros/foxy/setup.bash
$ ros2 launch slamcore_slam dataset_recorder.launch.py \
> override_realsense_depth:=true \
> realsense_depth_override_value:=true \
> output_dir:=<output-dir>
Warning
The depth stream is disabled by default on the NVIDIA Jetson platforms,
and must be turned on with the --depth
flag. You do not need to provide
the flag on Ubuntu 18.04 (x64) machines as it is enabled by default.
Step 2 - Prepare the mapping configuration file
When you install the “SLAMcore Tools” packages from the SLAMcore Portal, the
configuration files are automatically saved under the /usr/share/slamcore/presets
directory. You may use the presets best suited for your environment based on the description in
Configuration Overview, or further tune the parameters to create a custom configuration for your use
case.
We recommend creating a new configuration file combining the settings in the high_accuracy
,
mapping/default
and indoor_ground_robot
JSON files that are suitable for generating
a high accuracy map, and further tune it based on your setup.
To do this, create a new file high_accuracy_mapping.json
in a convenient directory, e.g.
~/slamcore/custom/high_accuracy_mapping.json
.
Edit the file by running this in a terminal window:
$ gedit ~/slamcore/custom/high_accuracy_mapping.json
Edit or paste the following text into the editor, and set DoTrackingOnPlane
to true
if the dataset was recorded on a wheeled platform and in a flat, single-level ground plane
environment.
High Accuracy Mapping Configuration
{
"Version": "1.0.0",
"Base":
{
"ProfileName": "high_accuracy_mapping",
"EnableFeatureMap2D": true
},
"Position":
{
"Estimator":
{
"NumKeyFrames": 9,
"NumIMUFrames": 5,
"TimeBudget": "33ms"
},
"Frontend":
{
"NumKeypoints": 250,
"MaxDepthUncertaintyRatio": 2.0,
"RadiusANMS": 11.0,
"TrackingOnPlane":
{
"DoTrackingOnPlane": false //set to true if using a wheeled platform on flat ground plane
}
},
"MultiSession":
{
"GBAUseInertialFactors": true
}
},
"Mapping":
{
"HeightMapper":
{
"MaxMapDimensions": 100.0,
"CellSize": 0.05,
"OccupancyExtraction":
{
"Method": "Gradient",
// Occupancy configuration for when "Method"=="Height"
"Height":
{
"HeightThreshold": -0.25
},
// Occupancy configuration for when "Method"=="Gradient"
"Gradient":
{
"GradientThreshold": 1.0
}
}
}
}
}
Additionally, if you have obtained a VIK configuration file, you may also include these parameters in the configuration file shown above.
Step 3 - Create a session map
Note
You may skip Step 1 and instead run SLAM live in this step to visualise the building of the height map as you travel through the test environment. However, we recommend recording a dataset first as that allows you to further tune parameters in the configuration file for the SLAM algorithm and the map settings.
To enable mapping in the software tools, use the flag -m
or --generate-map2d
.
The tools will process the dataset or live camera feed. Once the data is processed, you
can save a session file, which will contain the sparse map and height map.
For this step, you may use any of the following tools:
Using the SLAMcore Visualiser
$ slamcore_visualiser dataset -u <path/to/dataset> -m -c <path/to/config/file>
Press START
and wait for the dataset to be processed. Once done, press the SAVE
button and save the session file.
Note
See more information about the Visualiser’s user interface for mapping in the Height Mapping Mode section.
Using the CLI Dataset Processor
With the CLI Dataset Processor, you must also enable session saving with the -s
flag and specify an output directory:
$ slamcore_dataset_processor dataset -u <path/to/dataset> -m -s -c <path/to/config/file> -o <output/directory>
Note
You cannot process EuRoC dataset as they don’t offer any depth information.
Using ROS1
$ source /opt/ros/melodic/setup.bash
$ roslaunch slamcore_slam run_slam.launch dataset_path:=<path/to/dataset> config_file:=<full/path/to/config/file> session_save_dir:=<output/directory> generate_map2d:=true
# To enable ROS visualisation, run in another terminal or machine (optional)
$ roslaunch slamcore_viz setup_monitoring.launch
# Save the session once the dataset is fully processed
$ rosservice call /slamcore/save_session
Warning
Memory consumption is expected to grow constantly (but relatively slowly) during mapping.
Using ROS2
$ source /opt/ros/foxy/setup.bash
$ ros2 launch slamcore_slam slam_publisher.launch.py \
> dataset_path:=<path/to/dataset> \
> config_file:=<full/path/to/config/file> \
> session_save_dir:=<output/directory> \
> generate_map2d:=true
# Save the session once the dataset is fully processed
$ ros2 service call /slamcore/save_session std_srvs/srv/Trigger
Warning
Memory consumption is expected to grow constantly (but relatively slowly) during mapping.
Step 4 - Inspect Map in Session Explorer
Once the session map is obtained, you may use our Session Explorer tool to inspect and/or edit the map. To start the tool, run:
slamcore_session_explorer
Select File
and Open
and choose the .session
file to load.
Step 4.1 - Map Editing
There are two ways to edit the occupancy map that has been automatically generated in the session file: by tuning the map extraction, or by updating it with a manually edited image file.
Step 4.1.1 - Tuning Map Extraction
With the
Occupancy
channel selected, on the left sidebar, select theFrom height map
button to edit the map extraction method. Two options are provided: “Gradient” or “Height”. You may tweak the settings for different results in the occupancy map generated. For more information see Occupancy Map.Once satisfied with the changes, click
OK
to confirm orCancel
to reset. You can also reset to the default configuration later with theReset
button.
Step 4.1.2 - Import Edited Map
The Session Explorer tool also provides a functionality to update the session file by importing an edited occupancy map image. This is useful to map “unknown” areas missed during the session map generation and add or remove obstacles and walls from the map.
Select
File
andExport occupancy png
to export the current map as an image.Load the image in an image editor software (e.g. GIMP).
Using a pencil tool with a hard brush edge, manually edit the map using only three colours:
Occupied Pixel: Black RGB(0, 0, 0)
Free Pixel: White RGB(255, 255, 255)
Unknown Pixel: Grey RGB(128, 128, 128)
Save the image as an 8-bit Grayscale PNG.
In the Session Explorer tool, with the channel set to
Occupancy
, select theFrom file
button on the left side bar and import the edited image. The map will automatically be updated.
Warning
The Session Explorer tool does not support other image formats, a different image size or resolution, soft edges, or colours and values not mentioned above.
Step 4.1.3 - Saving Session
Once satisfied with the map, select File
and Save .session
to save the session
map. This edited map can be used in ROS for navigation.
Step 5 - Load Occupancy Map in ROS
To load the occupancy map in ROS1, parse a session file that contains a height/occupancy map:
$ source /opt/ros/melodic/setup.bash
$ roslaunch slamcore_slam run_slam.launch session_file:=<path/to/session>
# To enable ROS visualisation, run in another terminal or machine (optional)
$ roslaunch slamcore_viz setup_monitoring.launch
To load the occupancy map in ROS2, parse a session file that contains a height/occupancy map:
$ source /opt/ros/foxy/setup.bash
$ ros2 launch slamcore_slam slam_publisher.launch.py session_file:=<path/to/session>
You can now use the occupancy map for obstacle avoidance and robot navigation.