%% 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.
%% Open the App
% In the Apps tab, under  Control System Design and Analysis, click SLAM Map Builder.
% Also, you can call the function:
% slamMapBuilder
%
%% Import Lidar Scans from rosbag
% Click Import > Import from rosbag to load a rosbag. The provided rosbag, southend.bag, contains laser scan messages. Select the file and open. The scans are shown in the Import tab.
%
%
%
% In the Import Tab, specify the import parameters. The Lidar Topic is preselected as /scan because that is the only sensor_msgs/LaserScan topic found. If odometry is available as a tf tree, select Use TF, and specify the Lidar Frame (sensor frame) and the Fixed Frame (world frame).
% Select the desired Start Time (s) and End Time (s). Because the scans are captured at a high frequency, downsample the scans to reduce data processing. Select the desired percentage of scans in Downsample Scans to (%). Scans are evenly sampled. For example, 5% is every 20th scan.
% Click Apply to apply filtering parameters.
%
%
% Use the slider or arrow keys as the bottom to preview the scans.
%
%
% Once you are done filtering, 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 robotic 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.
%
%
% 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 example, we manually created a bad loop closure that does not normally occur with this data set at scan 218.
% 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.
%
% Copyright 2018 The MathWorks, Inc.
%
%