Map Indoor Area Using Lidar SLAM and Factor Graph

This example shows how to use the lidarscanmap (Lidar Toolbox) and factorGraph objects to implement the lidar simultaneous localization and mapping (SLAM) algorithm on a collected series of lidar scans of an indoor area. The goal of this example is to build a map of the indoor area using the lidar scans and retrieve the trajectory of the robot.

To build the map of the area, the SLAM algorithm incrementally processes the lidar scans and builds a lidar scan map, and a factor graph links these scans. The robot recognizes previously-visited places through scan matching, and uses them to establish loop closures along its moving path. The factor graph utilizes the loop closure information to optimize the trajectory of the robot and update the lidar scan map.

Load Laser Scan and Odometry Data from File

Load a downsampled data set consisting of laser scans and pose data collected by a Jackal™ robot from Clearpath Robotics™ in an indoor environment. The data includes a variable, messages, that contains lidar scans from the ROS topic /scan and poses from the topic /odom/filtered whose timestamps are closest to those of the scans from /scan. The original data set includes more than 2000 messages, which can take a long time to process. Therefore, improve performance, the raw data set has been downsampled to include only 111 messages, which is still enough to provide a good result. After downsampling, the average displacement between each pair of consecutive scans is around 0.45 meters.

Load the IndoorLidarScanAndOdometryData.mat file, which contains the messages variable, into the workspace.

load("IndoorLidarScanAndOdometryData.mat")

This floor plan and approximate path of the robot is for illustrative purposes, showing the indoor area to be mapped and the approximate trajectory of the robot.

Run Lidar SLAM Algorithm

To run the Lidar SLAM algorithm, you must construct the lidar scan map using the parameters from the lidar sensor on the robot. Then, create and use a factor graph to relate relative poses and optimize the graph.

Construct Lidar Scan Map

Create a 2-D lidarscanmap object with a maximum lidar range of 8 meters and a grid map resolution of 20 cells per meter, resulting in 5 centimeter precision. The robot is equipped with a 2-D laser scanner SICK™ TiM-511 with a max range of 10 meters, but laser readings are less accurate when detecting objects near the maximum range, so limiting the maximum range to 8 meters results in more accurate readings.

MaxLidarRange = 8;
MapResolution = 20;
scanMapObj = lidarscanmap(MapResolution,MaxLidarRange);

Add the first scan to the lidarscanmap object.

addScan(scanMapObj,messages{1}.scan);

Collect scan data for use in visualization.

scans = cell(length(messages),1);
scans{1} = messages{1}.scan;
firstTimeLCDetected = false;

Construct Factor Graph

Next, you must add scans to the lidar scan map. When you successfully add a scan, the object automatically calculates the relative pose. However, due to sensor noise and sharp turns, each measurement has an amount of error, called drift. This error accumulates as you extend the trajectory. To help mitigate this drift, use a factorGraph object to store the relative poses.

Create the factor graph and factor graph solver options objects.

fg = factorGraph;
opts = factorGraphSolverOptions;

To optimize drift correction, the factor graph requires more than just relative poses. The factor graph must also have some exact known poses to fix the relative poses in space, and to recognize when the robot returns to a previously visited location. A loop closure is when the algorithm detects that the robot has returned to a previous location. By detecting the loop closure you can add an additional constraint between the latest pose and a previous pose. The detectLoopClosure (Lidar Toolbox) object function of lidarscanmap searches for a previous scan that matches the most recent scan of the lidarscanmap object to check if the current pose creates a loop closure in the map. Note that, if the sample rate of the lidar sensor is high enough, the current pose can be close enough in time to a recent pose that the detectLoopClosure function to falsely detects the current pose as a loop closure. To avoid this, exclude a number of the most recent scans from loop closure detection.

Set the number of recent scans to exclude from loop closure detection to 30.

numExcludeViews = 30;

To increase the precision of the detected loop closure, you must tune the loop-closure match threshold and loop closure search radius values. The loop closure match threshold is the minimum correlation score that a scan must have to be considered a match for loop closure, and the loop closure search radius is the maximum distance that detectLoopClosure can search from the current scan to find a loop closure. This example sets these values empirically, but using a higher loop closure match threshold generally helps reject false positives in the loop closure identification process.

Set the loop closure match threshold to 185 and the loop closure search radius to 8 meters.

lcMatchThreshold = 185;
lcSearchRadius = 8;

For each of the scans:

1. Use the addScan (Lidar Toolbox) function to add the scans to the lidarscanmap object. The addScan function rejects scans if they are too close in distance for consecutive scans to ensure the lidar data is accurate.

2. If the lidar scan map accepts the new scan, create an SE(2) relative pose factor to connect the current pose and the previous pose. The relative pose factor is the 2-D relative pose between the current pose and previous pose. Then, add the factor to the factor graph to create a new node for the current pose and connect it to the previous pose in the factor graph.

3. Update the state of the current pose node with a pose guess from the odometry data at that time step.

4. If the scan is not one of the initial excluded recent scans, use the detectLoopClosure function to check if the current scan is a match for loop-closure. If the scan is a match, then detectLoopClosure returns the pose and pose ID of the matching scan.

5. If there is a loop closure, create another relative pose factor between the current pose and the pose that is a loop closure match. Then add that factor to the factor graph.

6. Fix the first node in the factor graph, and then optimize the graph.

7. Get all of the pose IDs in the factor graph and their post-optimization states.

8. For the first loop closure, use the exampleHelperPlotLidarScanMap helper function to visualize the first detected loop closure as a green line before and after optimization.

9. Use the updateScanPoses (Lidar Toolbox) object function to incrementally update the scans in the lidar scan map with the optimized states to get a better pose estimation for relative poses and loop closures.

for i = 2:length(messages)
    % Try to add scan to lidar scan map
    isScanAccepted = addScan(scanMapObj,messages{i}.scan);
    scans{i} = messages{i}.scan;

    % If map accepts scan
    if isScanAccepted
        % Set IDs for previous node and current node
        prevPoseID = scanMapObj.NumScans-1;
        currPoseID = scanMapObj.NumScans;

        % Get relative 2-D measurement between current and previous pose
        relPoseMeasure = scanMapObj.ConnectionAttributes.RelativePose(end,:);

        % Create 2-D relative pose factor and add it to factor graph
        poseFactor = factorTwoPoseSE2([prevPoseID currPoseID],Measurement=relPoseMeasure);
        addFactor(fg,poseFactor);

        % Update current pose node with initial pose guess
        nodeState(fg,currPoseID,messages{i}.Pose);

        % If scan is not one of initial excluded scans
        if i > numExcludeViews

            % Detect any new loop closure in lidar scan map
            [lcPose,lcPoseID] = detectLoopClosure(scanMapObj, ...
                                                  MatchThreshold=lcMatchThreshold, ...
                                                  SearchRadius=lcSearchRadius, ...
                                                  NumExcludeViews=numExcludeViews);
            % If there is a loop closure
            if ~isempty(lcPose)
                % Create relative pose factor between current pose
                % and previous pose that is a loop closure match
                lcFactor = factorTwoPoseSE2([lcPoseID currPoseID],Measurement=lcPose);
                addFactor(fg,lcFactor);

                % Fix the initial node, and optimize the factor graph
                fixNode(fg,1); 
                optimize(fg,opts);

                % Get all pose node IDs in the factor graph, and use those IDs to extract
                % the optimized poses of the corresponding pose nodes
                poseIDs = nodeIDs(fg,NodeType="POSE_SE2");
                optimizedPoseStates = nodeState(fg,poseIDs);

                % Visualize the first loop closure before and after factor graph optimization
                if ~firstTimeLCDetected
                    exampleHelperPlotLidarScanMap(lcPoseID,scanMapObj,optimizedPoseStates);
                    firstTimeLCDetected = true;
                end

                % Update the lidar scan map with optimized pose states
                updateScanPoses(scanMapObj,optimizedPoseStates);
            end
        end
    end
end

In the plot of the scan map before optimization, note that the drift has caused two offset scans of the same wall near the bottom of the map. After optimizing the factor graph, the two scans of the wall are closer together because the loop closures have reconciled the sensed walls.

Visualize Constructed Map and Trajectory of Robot

Plot the final built map after you have added all scans to the lidarscanmap object with factor-graph-optimized poses.

figure
show(scanMapObj);
title("Final Built Map of Environment with Robot Trajectory")

This image shows the scans and robot trajectory overlaid on the original floor plan.

Note that the scan map closely matches the original floor plan once you have added all the scans and optimized the factor graph. Currently, this workflow uses the factorTwoPoseSE2 objects for factor graph optimization whenever detectLoopClosure detects a loop closure to reduce drift, but the factor graph supports the use of other factor objects to refine the poses if you have IMU or other sensor data.

Build Occupancy Grid Map

You can use the optimized scans and poses to generate an occupancyMap, which represents the environment as a probabilistic occupancy grid.

map = buildMap(scans,scanMapObj.ScanAttributes.AbsolutePose,MapResolution,MaxLidarRange);

Visualize the occupancy grid map populated with the laser scans and the optimized pose graph.

figure
show(map);
hold on
show(scanMapObj,ShowMap=false);
hold off
title("Occupancy Grid Map Built Using Lidar SLAM")