SLAM Map Builder

Build 2-D grid maps using lidar-based SLAM

Description

The SLAM Map Builder app loads recorded lidar scans and odometry sensor data to build a 2-D occupancy grid using simultaneous localization and mapping (SLAM) algorithms. Incremental scan matching aligns and overlays scans to build the map. Loop closure detection adjusts for drift of the vehicle odometry by detecting previously visited locations and adjusting the overall map. Sometimes, the scan matching algorithm and loop closure detection require manual adjustment. Use the app to manually align scans and modify loop closures to improve the overall map accuracy. You can also tune the SLAM algorithm settings to improve the automatic map building.

To use the app:

	To load rosbag log files, select Import > Import from rosbag. Select the rosbag file and click Open. This opens the Import tab. For more information, see Import and Filter a rosbag. To load data from the workspace, Import > Import from workspace. Select your Scans and Poses variables using the drop downs provided. You can also specify the variables in the slamMapBuilder function. See Programmatic Use.
	Use SLAM Settings to adjust the SLAM algorithm settings. Default values are provided, but your specific sensors and data may require tuning of these settings. The most important value to tune is the Loop Closure Threshold. For more information, see Tune SLAM Settings.
	Click Build to begin the SLAM map building process. The building process aligns scans in the map using incremental scan matching, identifies loop closures when visiting previous locations, and adjusts poses. Click Pause at any time during the map building process to manually align incremental scans or modify loop closures.
	Click Incremental Match to modify the relative pose of the currently selected frame and align the scan with the previous scan. Click Loop Closure to modify or ignore the detected loop closure for the current frame. Use the slider on the bottom to scroll back to areas where scan matching or loop closures are not accurate. You can modify any number of scans or loop closures. For more information, see Modify Increment Scans and Loop Closures.
	After modifying your map, click Sync to update all the poses in the scan map. The two options under Sync are Sync, which searches for new loop closures, or Sync Fast, which skips loop closure searching and just updates the scan map. For more information, see Sync the Map.
	When you are satisfied with how the map looks, click Export to OccupancyGrid to either export the map to an m-file or save the map in the workspace. The map is output as a 2-D probabilistic occupancy grid in an occupancyMap object.
	You can open existing app sessions you have saved using Open Session. When you are in the Map Builder tab, you can save your progress to an m-file using Save Session.

×

Open the SLAM Map Builder App

• MATLAB^® Toolstrip: In the Apps tab, under ROBOTICS AND AUTONOMOUS SYSTEMS, click SLAM Map Builder.

• MATLAB Command Window: Enter slamMapBuilder

Examples

Build and Tune a Map Using Lidar Scans with SLAM

The SLAM Map Builder app helps you build an occupancy grid from lidar scans using simultaneous localization and mapping (SLAM) algorithms. The map is built by estimating poses through scan matching and using loop closures for pose graph optimization. This example shows you the workflow for loading a rosbag of lidar scan data, filtering the data, and building the map. Tune the scan map by adjusting incremental scan matches and modifying loop closures.

Load Lidar Scan Data

Load the example .mat file into the workspace, which contains a variable, scans, as a cell array of lidarScan objects.

load slamLidarScans.mat

Open the App

In the Apps tab, under Robotics and Autonomous Systems, click SLAM Map Builder.

Also, you can call the slamMapBuilder function:

slamMapBuilder

Import Lidar Scans

Click Import > Import from workspace to load the scans. Data stored as a rosbag can be loaded with a ROS Toolbox license.

Select the scans variable in the Scans drop down.

In the tool bar, set Downsample to (%) to 10. Downsampling evenly samples from the data to reduce computation time for the SLAM algorithm. For this example, 10% is every 5th scan. Click Apply.

Use the slider or arrow keys at the bottom to preview the scans.

Once you are done importing, click Close.

Tune SLAM Settings

The SLAM algorithm can be tuned using the SLAM Settings dialog. The parameters should be adjusted based on your sensor specifications, the environment, and your application. For this example, increase Loop Closure Threshold from 200 to 300. This increased threshold decreases the likelihood of accepting and using a detected loop closure. Set the Optimization Interval to 10. With every 10th loop closure accepted, the pose graph is optimized to account for drift.

Build the Map

After filtering your data and setting the SLAM algorithm settings, click Build. The app begins processing scans to build the map. You should see the slider progressing and scans being overlaid in the map. The estimated robot trajectory is plotted on the same scan map. Incremental scan matches are shown in the Incremental Match pane. Whenever a loop closure is detected, the Loop Closure pane shows the two scans overlaid on each other.

Adjust Scan Matches or Loop Closures

At any time during the build process, if you notice the map is distorted or an incremental match or loop closure looks off, click Pause to select scans for adjustment. You can modify scans at the end of the build process as well. Navigate using the arrow keys or slider to the point in the file where the distortion first occurs. Click the Incremental Match or Loop Closure buttons to adjust the currently displayed scan poses. In this section, the bad loop closure is artificial and only for illustration purposes.

Click the Loop Closure button. This opens a tab for modifying the loop closure relative pose.

To ignore the loop closure completely, click Ignore. Otherwise, manually modify the relative scan pose until the scans line up.

Click Pan Scan or Rotate Scan, then click and drag in the figure to align the two scans. Click Accept when you are done. You can do this for multiple scans.

After you modify your scan poses for incremental matches and loop closures, click Sync to apply changes. SyncFast updates the map without searching for new loop closures and reduces computation time if you have already processed all the scans.

Export Occupancy Grid

Once you have synced your changes and finished building the map, you should see a fully overlaid scan map with a robot trajectory.

Click Export Occupancy Grid to get a final occupancy map of your environment as a occupancyMap object. Specify the variable name to export the map to the workspace. You can create a map from a subset of scans by scrolling back to the desired frame before exporting and selecting Up to currently selected scan.

Call show on the stored map to visualize the occupancy map.

show(myOccMap)

You can also save a SLAM Map Builder app session using the Save Session button. The app writes the current state of the app to a .mat file that can be loaded later using Open Session.

Related Examples

• Implement Simultaneous Localization And Mapping (SLAM) with Lidar Scans
• Implement Online Simultaneous Localization And Mapping (SLAM) with Lidar Scans

Parameters

Scans — Lidar scans to import from workspace

N-element cell array of lidarScan objects

Lidar scans to import from workspace, specified as N-element cell array of lidarScan objects.

Poses (Odometry) — Poses to import from workspace

N-by-3 matrix | N-element cell array of three-element row vectors

Poses to import from the workspace, specified as either an N-by-3 matrix or an N-element cell array of three-element row vectors. Each row of the N-by-3 matrix and each three-element row vector of the cell array represent a pose in the form [x y theta]. x and y comprise the position in meters and theta is the orientation in radians.

Example: [0 0 0; 1 1 pi/4; 2 2 pi/2]

Example: {[0 0 0],[1 1 pi/4],[2 2 pi/2]}

Programmatic Use

slamMapBuilder(bag)

slamMapBuilder(bag) opens the SLAM Map Builder app and imports the rosbag log file specified in bag, a BagSelection object created using the rosbag (ROS Toolbox) function. The app opens to the Import tab to filter the sensor data in your rosbag.

slamMapBuilder(sessionFile)

slamMapBuilder(sessionFile) opens the SLAM Map Builder app from a saved session file name, sessionFile. An app session file is created through the Save Session button in the app toolstrip.

slamMapBuilder(scans)

slamMapBuilder(scans) opens the SLAM Map Builder app and imports the scans specified in scans, a cell array of lidarScan objects. The app assumes you have prefiltered your scans and skips the import process. Click Build to start building the map.

slamMapBuilder(scans,poses)

slamMapBuilder(scans,poses) opens the SLAM Map Builder app and imports the scans and poses. scans is specified as a cell array of lidarScan objects. poses is a matrix of [x y theta] vectors that correspond to the poses of scans. The app assumes you have prefiltered your scans and skips the import process. Click Build to start building the map.

More About

Import and Filter a rosbag

When you click the Import button, specify the parameters for your rosbag and how you want to filter the data in the toolstrip. You must Apply your settings to see the scans updated in the figures.

• Select the ROS topic for the lidar scans and odometry (if available).

• In Odom Topic, if you select Use TF, specify the frame of the lidar scan sensor, Lidar Frame, and the base fixed frame of the vehicle, Fixed Frame. The items in the drop down menu are generated based on the available frames in the tf transformation tree of the rosbag. The app only supports /tf and /tf2 topics.

• Specify the Start Time and End Time if you want to trim data from rosbag. You can use the sliders or manually type in your time values.

• Select the desired downsample percentage of scans in Downsample to (%) . This evenly downsamples the scans based on the percentage. For example, a value of 25% would only select every fourth scan.

• Click Apply to see the new filtered scans and apply all settings. Close the tab when you are done.

If you'd like more control over filtering scans in the rosbag, import your rosbag into MATLAB using rosbag (ROS Toolbox). Filter the rosbag using select (ROS Toolbox). To open the app using your custom filtered rosbag, see Programmatic Use.

Tune SLAM Settings

To improve the automatic map building process, the SLAM algorithm has tunable parameters. Click SLAM Settings to tune the parameters. Use Lidar SLAM Parameters to affect different aspects of the scan alignment and loop closure detection processes. Also, tune the NLP Solver Parameters to change how the map optimization algorithm improves the overall map based on loop closures.

Lidar SLAM Parameters:

• Map Resolution (cells/m) –– Resolution of the map. The resolution affects the location accuracy of the scan alignment and defines the output size of the occupancy grid.

• Lidar Range [min,max] (m) –– Range of lidar sensor readings. When processing the lidar scans, readings outside of the lidar range are ignored.

• Loop Closure Threshold –– Unitless threshold for accepting loop closures. Depending on your lidar scans, the average loop closure score varies. If the build process does not find loop closures and the vehicle revisits locations in the map, consider lowering this threshold.

• Loop Closure Search Radius (m) –– Radius to search for loop closures. Based on the odometry pose, the algorithm searches for loop closures in the existing map within the given radius in meters.

• Loop Closure Max Attempts –– Number of attempts at finding loop closures. When this number increases, the algorithm makes more attempts to find loop closures in the map but increases computation time.

• Loop Closure Auto Rollback –– Allow automatic rejection of loop closures. The algorithm tracks the residual error from the map optimization. If it detects a sudden change in the error and this parameter is set to on, the loop closure is rejected.

• Optimization Interval (# of Loop Closures) –– Number of detected loop closures accepted to trigger optimization. By default, the map is optimized with every loop closure found.

• Movement Threshold [Linear,Angular] (m,rad) –– Minimum change in pose required to accept a new scan. If the pose of the vehicle does not exceed this threshold, the next scan is discarded from the map building process.

NLP Solver Parameters:

• Max Iterations –– Maximum number of iterations for map optimization. Increasing this value may improve map accuracy but increases computation time.

• Max Time (s) –– Maximum time allowed for map optimization specified in seconds. Increasing this value may improve map accuracy but increases computation time.

• Gradient Tolerance –– Lower bound on the norm of the gradient of the cost function for optimization. Lowering this value causes the optimization to run longer to search for a local minimum but increases the computation time.

• Function Tolerance –– Lower bound on the change in the cost function for optimization. Lowering this value causes the optimization to run longer to search for a local minimum but increases the computation time.

• Step Tolerance –– Lower bound on the step size for optimization. Lowering this value causes the optimization to run longer to search for a local minimum but increases the computation time.

• First Node Pose [x,y,theta] (m,rad) –– Pose of the first node in the graph. If you need to offset the position of the scans in the map, specify the position, [x y], in meters and orientation, theta, in radians.

After changing any of these settings, the map building process must be restarted to rebuild the map with the new parameters.

Modify Incremental Matches and Loop Closures

This app allows you to manually modify incremental scans and adjust detected loop closures. If you notice scans are not properly aligned after you build the map, use the Incremental Match and Loop Closure buttons to open their modification tabs. Use the modification toolstrip buttons to adjust the relative pose between scans.

• Ignore –– When modifying loop closures, you can simply ignore loop closures if they are inaccurate. The algorithm always discards ignored loop closure if detected in the same app session. You cannot ignore incremental scan matches.

• Pan Scan –– Click this button to manually shift the pose. After selecting, click and drag inside the map to shift the scans and overlay them properly. Align all the points of the scans until you are satisfied. You can manually specify the X, Y location in the Relative Pose section as well.

• Rotate Scan –– Click this button to manually rotate the pose. After selecting, click and drag inside the map to rotate the scans and overlay them properly. Align all the points of the scans until you are satisfied. You can manually specify the Theta location in the Relative Pose section as well.

Sync the Map

After making modifications to the map building process using Incremental Scans and Loop Closures, you must sync the map to apply the changes. Based on the changes you make to properly align scans, the overall map shifts and alignments change for every scan after your modification. You have two options after making your modifications, Sync or Sync Fast. If you click Sync Fast, the changes to the poses are automatically applied and no other changes to the map occur. Sync restarts the entire map building and loop closure detection processes starting at the first modification. The specified modifications are applied, but the algorithm attempts to realign other scans and search for new loop closures as well.

Version History

Introduced in R2019b